diff --git a/.github/workflows/trigger-python.yml b/.github/workflows/trigger-python.yml new file mode 100644 index 0000000000..8fad9e7cad --- /dev/null +++ b/.github/workflows/trigger-python.yml @@ -0,0 +1,14 @@ +# This triggers Cython builds on `gtsam-manylinux-build` +name: Trigger Python Builds +on: push +jobs: + triggerCython: + runs-on: ubuntu-latest + steps: + - name: Repository Dispatch + uses: ProfFan/repository-dispatch@master + with: + token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }} + repository: borglab/gtsam-manylinux-build + event-type: cython-wrapper + client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' diff --git a/.travis.python.sh b/.travis.python.sh index 1ef5799aa4..772311f38e 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 bc90295957..535a72f4b8 100755 --- a/.travis.sh +++ b/.travis.sh @@ -1,5 +1,37 @@ #!/bin/bash +# install TBB with _debug.so files +function install_tbb() +{ + TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download + TBB_VERSION=4.4.2 + TBB_DIR=tbb44_20151115oss + TBB_SAVEPATH="/tmp/tbb.tgz" + + if [ "$TRAVIS_OS_NAME" == "linux" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="intel64/gcc4.4" + SUDO="sudo" + + elif [ "$TRAVIS_OS_NAME" == "osx" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="" + SUDO="" + + fi + + wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH + tar -C /tmp -xf $TBB_SAVEPATH + + TBBROOT=/tmp/$TBB_DIR + # Copy the needed files to the correct places. + # This works correctly for travis builds, instead of setting path variables. + # This is what Homebrew does to install TBB on Macs + $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/ + $SUDO cp -R $TBBROOT/include/ /usr/local/include/ + +} + # common tasks before either build or test function configure() { @@ -14,6 +46,8 @@ function configure() rm -fr $BUILD_DIR || true mkdir $BUILD_DIR && cd $BUILD_DIR + install_tbb + if [ ! -z "$GCC_VERSION" ]; then export CC=gcc-$GCC_VERSION export CXX=g++-$GCC_VERSION @@ -24,11 +58,12 @@ function configure() -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -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 fe9667330c..d8094ef4d7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,11 +14,12 @@ addons: - clang-9 - build-essential pkg-config - cmake - - libpython-dev python-numpy + - python3-dev libpython-dev + - python3-numpy - libboost-all-dev # before_install: -# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi + # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi @@ -89,6 +90,12 @@ jobs: compiler: clang env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON script: bash .travis.sh -b +# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests + - stage: special + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON + script: bash .travis.sh -t # -------- STAGE 2: TESTS ----------- # on Mac, GCC - stage: test diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ac85bff6f..edefbf2ea5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 0) -set (GTSAM_VERSION_PATCH 2) +set (GTSAM_VERSION_PATCH 3) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") @@ -22,6 +22,7 @@ set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include(GtsamMakeConfigFile) +include(GNUInstallDirs) # Record the root dir for gtsam - needed during external builds, e.g., ROS set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) @@ -67,8 +68,8 @@ if(GTSAM_UNSTABLE_AVAILABLE) endif() option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) @@ -199,12 +200,17 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) + set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) else() - set(GTSAM_USE_TBB 0) # This will go into config.h + set(GTSAM_USE_TBB 0) # This will go into config.h endif() ############################################################################### @@ -416,6 +422,10 @@ add_subdirectory(CppUnitLite) # Build wrap if (GTSAM_BUILD_WRAP) add_subdirectory(wrap) + # suppress warning of cython line being too long + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") + endif() endif(GTSAM_BUILD_WRAP) # Build GTSAM library @@ -444,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() @@ -527,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 ") @@ -585,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/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index f528412745..ab884ec1d3 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -12,6 +12,6 @@ gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON) if (GTSAM_INSTALL_CPPUNITLITE) - install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite) - install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION lib) + install(FILES ${cppunitlite_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/CppUnitLite) + install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) endif(GTSAM_INSTALL_CPPUNITLITE) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index a6cfee984b..953357eded 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/appveyor.yml b/appveyor.yml index 2c78ca1f24..3747354cf9 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 4.0.2-{branch}-build{build} +version: 4.0.3-{branch}-build{build} os: Visual Studio 2019 diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index 32e183baa8..15a15f3ed1 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -115,7 +115,7 @@ IF(WIN32 AND MKL_ROOT_DIR) IF (MKL_INCLUDE_DIR MATCHES "10.3") SET(MKL_LIBS ${MKL_LIBS} libiomp5md) ENDIF() - + FOREACH (LIB ${MKL_LIBS}) FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH) IF(${LIB}_PATH) @@ -147,7 +147,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} ${MKL_ROOT_DIR}/lib/ ) - + # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above) IF(NOT APPLE) FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY @@ -231,6 +231,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS FIND_LIBRARY(MKL_IOMP5_LIBRARY iomp5 PATHS + ${MKL_ROOT_DIR}/lib/intel64 ${MKL_ROOT_DIR}/../lib/intel64 ) ELSE() @@ -254,7 +255,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS ELSE() SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) ENDIF() - + MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) ENDIF() @@ -266,4 +267,4 @@ find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES) # LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack #endif() -MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES) \ No newline at end of file +MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 15a02b6e87..088ba7ad2b 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 e963a0d9c1..c8f8768959 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) @@ -87,6 +86,15 @@ endfunction() # - output_dir: The output directory function(build_cythonized_cpp target cpp_file output_lib_we output_dir) add_library(${target} MODULE ${cpp_file}) + + if(WIN32) + # Use .pyd extension instead of .dll on Windows + set_target_properties(${target} PROPERTIES SUFFIX ".pyd") + + # Add full path to the Python library + target_link_libraries(${target} ${PYTHON_LIBRARIES}) + endif() + if(APPLE) set(link_flags "-undefined dynamic_lookup") endif() @@ -129,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 @@ -141,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 @@ -166,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 # @@ -223,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}" - 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() + file(COPY "${source_directory}" DESTINATION "${dest_directory}" + FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endfunction() - diff --git a/cmake/example_cmake_find_gtsam/main.cpp b/cmake/example_cmake_find_gtsam/main.cpp index 4d93e1b19f..2904e00ddb 100644 --- a/cmake/example_cmake_find_gtsam/main.cpp +++ b/cmake/example_cmake_find_gtsam/main.cpp @@ -33,7 +33,6 @@ // Here we will use Between factors for the relative motion described by odometry measurements. // We will also use a Between Factor to encode the loop closure constraint // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -69,7 +68,7 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + graph.addPrior(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index a351ec52b1..2210255755 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -3,11 +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() + + # 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") - # wrap gtsam + # 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 @@ -15,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 @@ -25,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 bc6e346d9d..f69b7a5a6f 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. -## Install +## 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 + ``` -- 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: +- 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 -```bash - pip install -r /cython/requirements.txt -``` +- 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. -- 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. +- Build GTSAM and the wrapper with `make`. -- 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 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. -- 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 - -- Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH} - + so that it can find gtsam Cython header: gtsam/gtsam.pxd - -- 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") - -# 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. -``` -## KNOWN ISSUES + Examples: + + ```python + noiseBase = factor.noiseModel() + noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) + ``` - - 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? +## Wrapping Custom GTSAM-based Project +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? ### 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/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 6cc8979f1e..0e01766e7a 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -5,32 +5,27 @@ See LICENSE for the license information -A script validating the ImuFactor inference. +A script validating and demonstrating the ImuFactor inference. + +Author: Frank Dellaert, Varun Agrawal """ from __future__ import print_function import math +import gtsam import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand_B as B +from gtsam import symbol_shorthand_V as V +from gtsam import symbol_shorthand_X as X +from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D -import gtsam -from gtsam.utils.plot import plot_pose3 from PreintegrationExample import POSES_FIG, PreintegrationExample -BIAS_KEY = int(gtsam.symbol(ord('b'), 0)) - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol(ord('x'), key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol(ord('v'), key) +BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) @@ -76,8 +71,14 @@ def run(self): initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): state_i = self.scenario.navState(float(i)) - initial.insert(X(i), state_i.pose()) - initial.insert(V(i), state_i.velocity()) + + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + pose = state_i.pose().compose(poseNoise) + + velocity = state_i.velocity() + np.random.randn(3)*0.1 + + initial.insert(X(i), pose) + initial.insert(V(i), velocity) # simulate the loop i = 0 # state index @@ -88,6 +89,12 @@ def run(self): measuredAcc = self.runner.measuredSpecificForce(t) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + + actual_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3)*0.1) + # Plot IMU many times if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) @@ -133,6 +140,9 @@ def run(self): pose_i = result.atPose3(X(i)) plot_pose3(POSES_FIG, pose_i, 0.1) i += 1 + + gtsam.utils.plot.set_axes_equal(POSES_FIG) + print(result.atimuBias_ConstantBias(BIAS_KEY)) plt.ioff() diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 1f6732f499..0d98f08fea 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -8,23 +8,20 @@ import math +import gtsam +import gtsam.utils.plot as gtsam_plot import matplotlib.pyplot as plt import numpy as np +from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, + ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, + PinholeCameraCal3_S2, Point3, Pose3, + PriorFactorConstantBias, PriorFactorPose3, + PriorFactorVector, Rot3, Values) +from gtsam import symbol_shorthand_B as B +from gtsam import symbol_shorthand_V as V +from gtsam import symbol_shorthand_X as X from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 -import gtsam -import gtsam.utils.plot as gtsam_plot - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol(ord('x'), key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol(ord('v'), key) - def vector3(x, y, z): """Create 3d double numpy array.""" @@ -69,8 +66,8 @@ def ISAM2_plot(values, fignum=0): PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) -DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), - gtsam.Point3(0.05, -0.10, 0.20)) +DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), + Point3(0.05, -0.10, 0.20)) def IMU_example(): @@ -78,10 +75,10 @@ def IMU_example(): # Start with a camera on x-axis looking at origin radius = 30 - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - position = gtsam.Point3(radius, 0, 0) - camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) + up = Point3(0, 0, 1) + target = Point3(0, 0, 0) + position = Point3(radius, 0, 0) + camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses @@ -90,37 +87,37 @@ def IMU_example(): angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) - scenario = gtsam.ConstantTwistScenario( + scenario = ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph - newgraph = gtsam.NonlinearFactorGraph() + newgraph = NonlinearFactorGraph() # Create (incremental) ISAM2 solver - isam = gtsam.ISAM2() + isam = ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth - initialEstimate = gtsam.Values() + initialEstimate = Values() # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) - newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) + newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors - biasKey = gtsam.symbol(ord('b'), 0) + biasKey = B(0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) - biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), - biasnoise) + biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), + biasnoise) newgraph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity n_velocity = vector3(0, angular_velocity * radius, 0) - velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) + velprior = PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) @@ -141,7 +138,7 @@ def IMU_example(): # Add Bias variables periodically if i % 5 == 0: biasKey += 1 - factor = gtsam.BetweenFactorConstantBias( + factor = BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) newgraph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) @@ -154,8 +151,7 @@ def IMU_example(): accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor - imufac = gtsam.ImuFactor( - X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong @@ -168,7 +164,7 @@ def IMU_example(): ISAM2_plot(result) # reset - newgraph = gtsam.NonlinearFactorGraph() + newgraph = NonlinearFactorGraph() initialEstimate.clear() diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index fb038d043f..c84f0f8344 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -13,9 +13,10 @@ from __future__ import print_function -import numpy as np - import gtsam +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X # Create noise models PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -26,11 +27,11 @@ graph = gtsam.NonlinearFactorGraph() # Create the keys corresponding to unknown variables in the factor graph -X1 = gtsam.symbol(ord('x'), 1) -X2 = gtsam.symbol(ord('x'), 2) -X3 = gtsam.symbol(ord('x'), 3) -L1 = gtsam.symbol(ord('l'), 4) -L2 = gtsam.symbol(ord('l'), 5) +X1 = X(1) +X2 = X(2) +X3 = X(3) +L1 = L(4) +L2 = L(5) # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 3aee7daff3..09114370dd 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py @@ -53,16 +53,15 @@ def vector3(x, y, z): assert args.kernel == "none", "Supplied kernel type is not yet implemented" # Add prior on the pose having index (key) = 0 -graphWithPrior = graph priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) +graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") params.setMaxIterations(maxIterations) # parameters.setRelativeErrorTol(1e-5) # Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) # ... and optimize result = optimizer.optimize() @@ -83,7 +82,7 @@ def vector3(x, y, z): print ("Done!") if args.plot: - resultPoses = gtsam.extractPose2(result) + resultPoses = gtsam.utilities_extractPose2(result) for i in range(resultPoses.shape[0]): plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) plt.show() diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index 38c5db275a..3c1a54f7b4 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py @@ -43,18 +43,17 @@ def vector6(x, y, z, a, b, c): 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") -graphWithPrior = graph firstKey = initial.keys().at(0) -graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) +graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") -print("initial error = ", graphWithPrior.error(initial)) -print("final error = ", graphWithPrior.error(result)) +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) @@ -66,7 +65,7 @@ def vector6(x, y, z, a, b, c): print ("Done!") if args.plot: - resultPoses = gtsam.allPose3s(result) + resultPoses = gtsam.utilities_allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show() diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 35ec4f66d8..99bce00e2d 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -1,7 +1,7 @@ These examples are almost identical to the old handwritten python wrapper examples. However, there are just some slight name changes, for example `noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc... -Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))` +Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing. # Porting Progress diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index bf46f09d51..e02def2f91 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -11,17 +11,17 @@ from __future__ import print_function import gtsam +import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X from gtsam.examples import SFMdata from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, - GenericProjectionFactorCal3_S2, NonlinearFactorGraph, - Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, - Rot3, SimpleCamera, Values) - - -def symbol(name: str, index: int) -> int: - """ helper for creating a symbol without explicitly casting 'name' from str to int """ - return gtsam.symbol(ord(name), index) + GenericProjectionFactorCal3_S2, Marginals, + NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, + Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, + SimpleCamera, Values) +from gtsam.utils import plot def main(): @@ -70,23 +70,23 @@ def main(): # Add a prior on pose x1. This indirectly specifies where the origin is. # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) # Simulated measurements from each camera pose, adding them to the factor graph for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2( - measurement, measurement_noise, symbol('x', i), symbol('l', j), K) + measurement, measurement_noise, X(i), L(j), K) graph.push_back(factor) # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) - factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) graph.print_('Factor Graph:\n') @@ -94,13 +94,11 @@ def main(): # Intentionally initialize the variables off from the ground truth initial_estimate = Values() for i, pose in enumerate(poses): - r = Rot3.Rodrigues(-0.1, 0.2, 0.25) - t = Point3(0.05, -0.10, 0.20) - transformed_pose = pose.compose(Pose3(r, t)) - initial_estimate.insert(symbol('x', i), transformed_pose) + transformed_pose = pose.retract(0.1*np.random.randn(6,1)) + initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): - transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15])) - initial_estimate.insert(symbol('l', j), transformed_point) + transformed_point = Point3(point.vector() + 0.1*np.random.randn(3)) + initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results @@ -113,6 +111,11 @@ def main(): print('initial error = {}'.format(graph.error(initial_estimate))) print('final error = {}'.format(graph.error(result))) + marginals = Marginals(graph, result) + plot.plot_3d_points(1, result, marginals=marginals) + plot.plot_trajectory(1, result, marginals=marginals, scale=8) + plot.set_axes_equal(1) + plt.show() if __name__ == '__main__': main() diff --git a/cython/gtsam/examples/SFMdata.py b/cython/gtsam/examples/SFMdata.py index 1d5c499fa0..c586f7e521 100644 --- a/cython/gtsam/examples/SFMdata.py +++ b/cython/gtsam/examples/SFMdata.py @@ -25,14 +25,15 @@ def createPoints(): def createPoses(K): # Create the set of ground-truth poses - radius = 30.0 + radius = 40.0 + height = 10.0 angles = np.linspace(0, 2*np.pi, 8, endpoint=False) up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) poses = [] for theta in angles: position = gtsam.Point3(radius*np.cos(theta), - radius*np.sin(theta), 0.0) + radius*np.sin(theta), height) camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) poses.append(camera.pose()) return poses diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py index 6e9b1320bd..4e82d37787 100644 --- a/cython/gtsam/examples/SimpleRotation.py +++ b/cython/gtsam/examples/SimpleRotation.py @@ -10,8 +10,9 @@ a single variable with a single factor. """ -import numpy as np import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X def main(): @@ -33,7 +34,7 @@ def main(): prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) prior.print_('goal angle') model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) - key = gtsam.symbol(ord('x'), 1) + key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) """ diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py index c0f4fbd948..49e6ca95c3 100644 --- a/cython/gtsam/examples/VisualISAM2Example.py +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -13,23 +13,14 @@ from __future__ import print_function -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - import gtsam import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X from gtsam.examples import SFMdata - - -def X(i): - """Create key for pose i.""" - return int(gtsam.symbol(ord('x'), i)) - - -def L(j): - """Create key for landmark j.""" - return int(gtsam.symbol(ord('l'), j)) +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def visual_ISAM2_plot(result): diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py index f4d81eaf74..5cc37867b7 100644 --- a/cython/gtsam/examples/VisualISAMExample.py +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -18,12 +18,9 @@ from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values) - - -def symbol(name: str, index: int) -> int: - """ helper for creating a symbol without explicitly casting 'name' from str to int """ - return gtsam.symbol(ord(name), index) + PinholeCameraCal3_S2, Values) +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X def main(): @@ -54,11 +51,11 @@ def main(): # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) # Add factors for each landmark observation for j, point in enumerate(points): measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K) + factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth @@ -66,7 +63,7 @@ def main(): initial_xi = pose.compose(noise) # Add an initial guess for the current pose - initial_estimate.insert(symbol('x', i), initial_xi) + initial_estimate.insert(X(i), initial_xi) # If this is the first iteration, add a prior on the first pose to set the coordinate frame # and a prior on the first landmark to set the scale @@ -75,12 +72,12 @@ def main(): if i == 0: # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) # Add a prior on landmark l0 point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) - factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) # Add initial guesses to all observed landmarks @@ -88,7 +85,7 @@ def main(): for j, point in enumerate(points): # Intentionally initialize the variables off from the ground truth initial_lj = points[j].vector() + noise - initial_estimate.insert(symbol('l', j), Point3(initial_lj)) + initial_estimate.insert(L(j), Point3(initial_lj)) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py new file mode 100644 index 0000000000..f3f5354bb3 --- /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_GaussianFactorGraph.py b/cython/gtsam/tests/test_GaussianFactorGraph.py index 2e7875af06..983825d8bc 100644 --- a/cython/gtsam/tests/test_GaussianFactorGraph.py +++ b/cython/gtsam/tests/test_GaussianFactorGraph.py @@ -15,17 +15,18 @@ import unittest import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X from gtsam.utils.test_case import GtsamTestCase -import numpy as np def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - x0 = gtsam.symbol(ord('x'), 0) - x1 = gtsam.symbol(ord('x'), 1) - x2 = gtsam.symbol(ord('x'), 2) + x0 = X(0) + x1 = X(1) + x2 = X(2) BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py index efefb218a7..985dc30a21 100644 --- a/cython/gtsam/tests/test_NonlinearOptimizer.py +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -17,7 +17,8 @@ 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 @@ def test_optimize(self): 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_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py index 0acf50c2cf..66207b8007 100644 --- a/cython/gtsam/tests/test_PriorFactor.py +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -35,5 +35,27 @@ def test_PriorFactor(self): values.insert(key, priorVector) self.assertEqual(factor.error(values), 0) + def test_AddPrior(self): + """ + Test adding prior factors directly to factor graph via the .addPrior method. + """ + # define factor graph + graph = gtsam.NonlinearFactorGraph() + + # define and add Pose3 prior + key = 5 + priorPose3 = gtsam.Pose3() + model = gtsam.noiseModel_Unit.Create(6) + graph.addPriorPose3(key, priorPose3, model) + self.assertEqual(graph.size(), 1) + + # define and add Vector prior + key = 3 + priorVector = np.array([0., 0., 0.]) + model = gtsam.noiseModel_Unit.Create(3) + graph.addPriorVector(key, priorVector, model) + self.assertEqual(graph.size(), 2) + + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index efdfec561b..a3654a5f17 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -5,7 +5,7 @@ See LICENSE for the license information -SimpleCamera unit tests. +PinholeCameraCal3_S2 unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import math @@ -14,7 +14,7 @@ import numpy as np import gtsam -from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) @@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) - camera = SimpleCamera(pose1, K) + camera = PinholeCameraCal3_S2(pose1, K) self.gtsamAssertEquals(camera.calibration(), K, 1e-9) self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction pose2 = Pose2(0.4,0.3,math.pi/2.0) - camera = SimpleCamera.Level(K, pose2, 0.1) + camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1) # expected x = Point3(1,0,0) diff --git a/cython/gtsam/tests/test_dsf_map.py b/cython/gtsam/tests/test_dsf_map.py index b2ede57bf0..73ddcb0505 100644 --- a/cython/gtsam/tests/test_dsf_map.py +++ b/cython/gtsam/tests/test_dsf_map.py @@ -33,7 +33,7 @@ def key(index_pair): # testing the merge feature of dsf dsf.merge(pair1, pair2) - self.assertEquals(key(dsf.find(pair1)), key(dsf.find(pair2))) + self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py new file mode 100644 index 0000000000..2560a72a23 --- /dev/null +++ b/cython/gtsam/tests/test_logging_optimizer.py @@ -0,0 +1,108 @@ +""" +Unit tests for optimization that logs to comet.ml. +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 + +import gtsam +import numpy as np +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + +from gtsam.utils.logging_optimizer import gtsam_optimize + +KEY = 0 +MODEL = gtsam.noiseModel_Unit.Create(3) + + +class TestOptimizeComet(GtsamTestCase): + """Check correct logging to comet.ml.""" + + def setUp(self): + """Set up a small Karcher mean optimization example.""" + # Grabbed from KarcherMeanFactor unit tests. + R = Rot3.Expmap(np.array([0.1, 0, 0])) + rotations = {R, R.inverse()} # mean is the identity + self.expected = Rot3() + + graph = gtsam.NonlinearFactorGraph() + for R in rotations: + graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) + initial = gtsam.Values() + initial.insert(KEY, R) + self.params = gtsam.GaussNewtonParams() + 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): + print(error) + + # Only thing we require from optimizer is an iterate method + gtsam_optimize(self.optimizer, self.params, hook) + + # Check that optimizing yields the identity. + 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.""" + from comet_ml import Experiment + comet = Experiment(project_name="Testing", + auto_output_logging="native") + comet.log_dataset_info(name="Karcher", path="shonan") + comet.add_tag("GaussNewton") + comet.log_parameter("method", "GaussNewton") + time = datetime.now() + comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " " + + str(time.hour)+":"+str(time.minute)+":"+str(time.second)) + + # I want to do some comet thing here + def hook(optimizer, error): + comet.log_metric("Karcher error", + error, optimizer.iterations()) + + gtsam_optimize(self.optimizer, self.params, hook) + comet.end() + + actual = self.optimizer.values() + self.gtsamAssertEquals(actual.atRot3(KEY), self.expected) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/utils/logging_optimizer.py b/cython/gtsam/utils/logging_optimizer.py new file mode 100644 index 0000000000..3d9175951a --- /dev/null +++ b/cython/gtsam/utils/logging_optimizer.py @@ -0,0 +1,52 @@ +""" +Optimization with logging via a hook. +Author: Jing Wu and Frank Dellaert +""" +# pylint: disable=invalid-name + +from gtsam import NonlinearOptimizer, NonlinearOptimizerParams +import gtsam + + +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. + Arguments: + optimizer (T): needs an iterate and an error function. + check_convergence: T * float * float -> bool + hook -- hook function to record the error + """ + # the optimizer is created with default values which incur the error below + current_error = optimizer.error() + hook(optimizer, current_error) + + # Iterative loop + while True: + # Do next iteration + optimizer.iterate() + new_error = optimizer.error() + hook(optimizer, new_error) + if check_convergence(optimizer, current_error, new_error): + return + current_error = new_error + + +def gtsam_optimize(optimizer, + params, + hook): + """ Given an optimizer and params, iterate until convergence. + After each iteration, hook(optimizer) is called. + After the function, use values and errors to get the result. + Arguments: + optimizer {NonlinearOptimizer} -- Nonlinear optimizer + params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters + hook -- hook function to record the error + """ + 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)) or ( + isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound()) + optimize(optimizer, check_convergence, hook) + return optimizer.values() diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 3871fa18c3..b67444fc14 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -3,10 +3,109 @@ import numpy as np import matplotlib.pyplot as plt from matplotlib import patches +from mpl_toolkits.mplot3d import Axes3D + +import gtsam + + +def set_axes_equal(fignum): + """ + Make axes of 3D plot have equal scale so that spheres appear as spheres, + cubes as cubes, etc.. This is one possible solution to Matplotlib's + ax.set_aspect('equal') and ax.axis('equal') not working for 3D. + + Args: + fignum (int): An integer representing the figure number for Matplotlib. + """ + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + + limits = np.array([ + ax.get_xlim3d(), + ax.get_ylim3d(), + ax.get_zlim3d(), + ]) + + origin = np.mean(limits, axis=1) + radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) + + ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) + ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) + ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) + + +def ellipsoid(xc, yc, zc, rx, ry, rz, n): + """ + Numpy equivalent of Matlab's ellipsoid function. + + Args: + xc (double): Center of ellipsoid in X-axis. + yc (double): Center of ellipsoid in Y-axis. + zc (double): Center of ellipsoid in Z-axis. + rx (double): Radius of ellipsoid in X-axis. + ry (double): Radius of ellipsoid in Y-axis. + rz (double): Radius of ellipsoid in Z-axis. + n (int): The granularity of the ellipsoid plotted. + + Returns: + tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. + """ + u = np.linspace(0, 2*np.pi, n+1) + v = np.linspace(0, np.pi, n+1) + x = -rx * np.outer(np.cos(u), np.sin(v)).T + y = -ry * np.outer(np.sin(u), np.sin(v)).T + z = -rz * np.outer(np.ones_like(u), np.cos(v)).T + + return x, y, z + + +def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): + """ + Plots a Gaussian as an uncertainty ellipse + + Based on Maybeck Vol 1, page 366 + k=2.296 corresponds to 1 std, 68.26% of all probability + k=11.82 corresponds to 3 std, 99.74% of all probability + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + origin (gtsam.Point3): The origin in the world frame. + P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. + scale (float): Scaling factor of the radii of the covariance ellipse. + n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. + alpha (float): Transparency value for the plotted surface in the range [0, 1]. + """ + k = 11.82 + U, S, _ = np.linalg.svd(P) + + radii = k * np.sqrt(S) + radii = radii * scale + rx, ry, rz = radii + + # generate data for "unrotated" ellipsoid + xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n) + + # rotate data with orientation matrix U and center c + data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \ + np.kron(U[:, 2:3], zc) + n = data.shape[1] + x = data[0:n, :] + origin[0] + y = data[n:2*n, :] + origin[1] + z = data[2*n:, :] + origin[2] + + axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): - """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" + """ + Plot a 2D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() @@ -35,32 +134,66 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): np.rad2deg(angle), fill=False) axes.add_patch(e1) + def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): - """Plot a 2D pose on given figure with given 'axis_length'.""" + """ + Plot a 2D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length, covariance) + plot_pose2_on_axes(axes, pose, axis_length=axis_length, + covariance=covariance) + +def plot_point3_on_axes(axes, point, linespec, P=None): + """ + Plot a 3D point on given axis `axes` with given `linespec`. -def plot_point3_on_axes(axes, point, linespec): - """Plot a 3D point on given axis 'axes' with given 'linespec'.""" + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ axes.plot([point.x()], [point.y()], [point.z()], linespec) + if P is not None: + plot_covariance_ellipse_3d(axes, point.vector(), P) -def plot_point3(fignum, point, linespec): - """Plot a 3D point on given figure with given 'linespec'.""" +def plot_point3(fignum, point, linespec, P=None): + """ + Plot a 3D point on given figure with given `linespec`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_point3_on_axes(axes, point, linespec) + plot_point3_on_axes(axes, point, linespec, P) -def plot_3d_points(fignum, values, linespec, marginals=None): +def plot_3d_points(fignum, values, linespec="g*", marginals=None): """ - Plots the Point3s in 'values', with optional covariances. + Plots the Point3s in `values`, with optional covariances. Finds all the Point3 objects in the given Values object and plots them. If a Marginals object is given, this function will also plot marginal covariance ellipses for each point. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dictionary consisting of points to be plotted. + linespec (string): String representing formatting options for Matplotlib. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ keys = values.keys() @@ -68,23 +201,33 @@ def plot_3d_points(fignum, values, linespec, marginals=None): # Plot points and covariance matrices for i in range(keys.size()): try: - p = values.atPoint3(keys.at(i)) - # if haveMarginals - # P = marginals.marginalCovariance(key); - # gtsam.plot_point3(p, linespec, P); - # else - plot_point3(fignum, p, linespec) + key = keys.at(i) + point = values.atPoint3(key) + if marginals is not None: + covariance = marginals.marginalCovariance(key) + else: + covariance = None + + plot_point3(fignum, point, linespec, covariance) + except RuntimeError: continue # I guess it's not a Point3 -def plot_pose3_on_axes(axes, pose, axis_length=0.1): - """Plot a 3D pose on given axis 'axes' with given 'axis_length'.""" +def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): + """ + Plot a 3D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global - t = pose.translation() - origin = np.array([t.x(), t.y(), t.z()]) + origin = pose.translation().vector() # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -100,17 +243,83 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1): axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') # plot the covariance - # TODO (dellaert): make this work - # if (nargin>2) && (~isempty(P)) - # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame - # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - # gtsam.covarianceEllipse3D(origin,gPp); - # end + if P is not None: + # covariance matrix in pose coordinate frame + pPp = P[3:6, 3:6] + # convert the covariance matrix to global coordinate frame + gPp = gRp @ pPp @ gRp.T + plot_covariance_ellipse_3d(axes, origin, gPp) + +def plot_pose3(fignum, pose, axis_length=0.1, P=None): + """ + Plot a 3D pose on given figure with given `axis_length`. -def plot_pose3(fignum, pose, axis_length=0.1): - """Plot a 3D pose on given figure with given 'axis_length'.""" + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose3): 3D pose to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get figure object fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, axis_length) + plot_pose3_on_axes(axes, pose, P=P, + axis_length=axis_length) + + +def plot_trajectory(fignum, values, scale=1, marginals=None): + """ + Plot a complete 3D trajectory using poses in `values`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dict containing the poses. + scale (float): Value to scale the poses by. + marginals (gtsam.Marginals): Marginalized probability values of the estimation. + Used to plot uncertainty bounds. + """ + pose3Values = gtsam.utilities_allPose3s(values) + keys = gtsam.KeyVector(pose3Values.keys()) + lastIndex = None + + for i in range(keys.size()): + key = keys.at(i) + try: + pose = pose3Values.atPose3(key) + except: + print("Warning: no Pose3 at key: {0}".format(key)) + + if lastIndex is not None: + lastKey = keys.at(lastIndex) + try: + lastPose = pose3Values.atPose3(lastKey) + except: + print("Warning: no Pose3 at key: {0}".format(lastKey)) + pass + + if marginals: + covariance = marginals.marginalCovariance(lastKey) + else: + covariance = None + + plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale) + + lastIndex = i + + # Draw final pose + if lastIndex is not None: + lastKey = keys.at(lastIndex) + try: + lastPose = pose3Values.atPose3(lastKey) + if marginals: + covariance = marginals.marginalCovariance(lastKey) + else: + covariance = None + + plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale) + + except: + pass diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py index 91194c5650..f04588e705 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -1,8 +1,9 @@ from __future__ import print_function import numpy as np -from math import pi, cos, sin + import gtsam +from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3 class Options: @@ -10,7 +11,7 @@ class Options: Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): + def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -27,10 +28,10 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.cameras = [gtsam.Pose3()] * nrCameras - self.points = [gtsam.Point3()] * nrPoints + self.cameras = [Pose3()] * nrCameras + self.points = [Point3()] * nrPoints def print_(self, s=""): print(s) @@ -52,11 +53,11 @@ class Data: class NoiseModels: pass - def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] + self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] - self.odometry = [gtsam.Pose3()] * nrCameras + self.odometry = [Pose3()] * nrCameras # Set Noise parameters self.noiseModels = Data.NoiseModels() @@ -73,7 +74,7 @@ def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): def generate_data(options): """ Generate ground-truth and measurement data. """ - K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) nrPoints = 3 if options.triangle else 8 truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) @@ -83,26 +84,26 @@ def generate_data(options): if options.triangle: # Create a triangle target, just 3 points on a plane r = 10 for j in range(len(truth.points)): - theta = j * 2 * pi / nrPoints - truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) + theta = j * 2 * np.pi / nrPoints + truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ - gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), - gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), - gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), - gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10) + Point3(10, 10, 10), Point3(-10, 10, 10), + Point3(-10, -10, 10), Point3(10, -10, 10), + Point3(10, 10, -10), Point3(-10, 10, -10), + Point3(-10, -10, -10), Point3(10, -10, -10) ] # Create camera cameras on a circle around the triangle height = 10 r = 40 for i in range(options.nrCameras): - theta = i * 2 * pi / options.nrCameras - t = gtsam.Point3(r * cos(theta), r * sin(theta), height) - truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, - gtsam.Point3(), - gtsam.Point3(0, 0, 1), - truth.K) + theta = i * 2 * np.pi / options.nrCameras + t = Point3(r * np.cos(theta), r * np.sin(theta), height) + truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, + Point3(), + Point3(0, 0, 1), + truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt index 77bead834d..a0cf0fbde9 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/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py new file mode 100644 index 0000000000..6ba06f0f23 --- /dev/null +++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -0,0 +1,128 @@ +""" +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 + +Track a moving object "Time of Arrival" measurements at 4 microphones. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, no-name-in-module + +from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic) +from gtsam_unstable import Event, TimeOfArrival, TOAFactor + +# units +MS = 1e-3 +CM = 1e-2 + +# Instantiate functor with speed of sound value +TIME_OF_ARRIVAL = TimeOfArrival(330) + + +def define_microphones(): + """Create microphones.""" + height = 0.5 + microphones = [] + microphones.append(Point3(0, 0, height)) + microphones.append(Point3(403 * CM, 0, height)) + microphones.append(Point3(403 * CM, 403 * CM, height)) + microphones.append(Point3(0, 403 * CM, 2 * height)) + return microphones + + +def create_trajectory(n): + """Create ground truth trajectory.""" + trajectory = [] + timeOfEvent = 10 + # simulate emitting a sound every second while moving on straight line + for key in range(n): + trajectory.append( + Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM)) + timeOfEvent += 1 + + return trajectory + + +def simulate_one_toa(microphones, event): + """Simulate time-of-arrival measurements for a single event.""" + return [TIME_OF_ARRIVAL.measure(event, microphones[i]) + for i in range(len(microphones))] + + +def simulate_toa(microphones, trajectory): + """Simulate time-of-arrival measurements for an entire trajectory.""" + return [simulate_one_toa(microphones, event) + for event in trajectory] + + +def create_graph(microphones, simulatedTOA): + """Create factor graph.""" + graph = NonlinearFactorGraph() + + # Create a noise model for the TOA error + model = noiseModel_Isotropic.Sigma(1, 0.5 * MS) + + K = len(microphones) + key = 0 + for toa in simulatedTOA: + for i in range(K): + factor = TOAFactor(key, microphones[i], toa[i], model) + graph.push_back(factor) + key += 1 + + return graph + + +def create_initial_estimate(n): + """Create initial estimate for n events.""" + initial = Values() + zero = Event() + for key in range(n): + TOAFactor.InsertEvent(key, zero, initial) + return initial + + +def toa_example(): + """Run example with 4 microphones and 5 events in a straight line.""" + # Create microphones + microphones = define_microphones() + K = len(microphones) + for i in range(K): + print("mic {} = {}".format(i, microphones[i])) + + # Create a ground truth trajectory + n = 5 + groundTruth = create_trajectory(n) + for event in groundTruth: + print(event) + + # Simulate time-of-arrival measurements + simulatedTOA = simulate_toa(microphones, groundTruth) + for key in range(n): + for i in range(K): + print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS)) + + # create factor graph + graph = create_graph(microphones, simulatedTOA) + print(graph.at(0)) + + # Create initial estimate + initial_estimate = create_initial_estimate(n) + print(initial_estimate) + + # Optimize using Levenberg-Marquardt optimization. + params = LevenbergMarquardtParams() + params.setAbsoluteErrorTol(1e-10) + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params) + result = optimizer.optimize() + print("Final Result:\n", result) + + +if __name__ == '__main__': + toa_example() + print("Example complete") diff --git a/cython/requirements.txt b/cython/requirements.txt index cd77b097d6..8d3c7aeb49 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 aefa112411..98a05c9f6d 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', '.dll')] - 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 74eb351cd8..0000000000 --- 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 ef5d5ab972..0000000000 --- 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 ec635144f6..0000000000 --- a/debian/compat +++ /dev/null @@ -1 +0,0 @@ -9 diff --git a/debian/control b/debian/control deleted file mode 100644 index 9b3ae53083..0000000000 --- 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 c2f41d83d5..0000000000 --- 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 e69de29bb2..0000000000 diff --git a/debian/rules b/debian/rules deleted file mode 100755 index 156629fd61..0000000000 --- a/debian/rules +++ /dev/null @@ -1,25 +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 - - -# 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=OFF -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 - diff --git a/debian/source/format b/debian/source/format deleted file mode 100644 index 163aaf8d82..0000000000 --- a/debian/source/format +++ /dev/null @@ -1 +0,0 @@ -3.0 (quilt) diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp index 6e736e2d7a..2befa9dc22 100644 --- a/doc/Code/OdometryExample.cpp +++ b/doc/Code/OdometryExample.cpp @@ -5,7 +5,7 @@ NonlinearFactorGraph graph; Pose2 priorMean(0.0, 0.0, 0.0); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); -graph.add(PriorFactor(1, priorMean, priorNoise)); +graph.addPrior(1, priorMean, priorNoise); // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp index 1738aabc5c..192ec11a4e 100644 --- a/doc/Code/Pose2SLAMExample.cpp +++ b/doc/Code/Pose2SLAMExample.cpp @@ -1,7 +1,7 @@ NonlinearFactorGraph graph; noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); -graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); +graph.addPrior(1, Pose2(0, 0, 0), priorNoise); // Add odometry factors noiseModel::Diagonal::shared_ptr model = diff --git a/doc/gtsam-coordinate-frames.lyx b/doc/gtsam-coordinate-frames.lyx index 33d0dd977e..cfb44696b3 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 3613ef0ac8..77910b4cf3 100644 Binary files a/doc/gtsam-coordinate-frames.pdf and b/doc/gtsam-coordinate-frames.pdf differ diff --git a/doc/math.lyx b/doc/math.lyx index b579d3ea41..2533822a73 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -1,7 +1,9 @@ -#LyX 2.1 created this file. For more info see http://www.lyx.org/ -\lyxformat 474 +#LyX 2.3 created this file. For more info see http://www.lyx.org/ +\lyxformat 544 \begin_document \begin_header +\save_transient_properties true +\origin unavailable \textclass article \use_default_options false \begin_modules @@ -14,16 +16,18 @@ theorems-ams-bytype \language_package default \inputencoding auto \fontencoding global -\font_roman times -\font_sans default -\font_typewriter default -\font_math auto +\font_roman "times" "default" +\font_sans "default" "default" +\font_typewriter "default" "default" +\font_math "auto" "auto" \font_default_family rmdefault \use_non_tex_fonts false \font_sc false \font_osf false -\font_sf_scale 100 -\font_tt_scale 100 +\font_sf_scale 100 100 +\font_tt_scale 100 100 +\use_microtype false +\use_dash_ligatures true \graphics default \default_output_format default \output_sync 0 @@ -53,6 +57,7 @@ theorems-ams-bytype \suppress_date false \justification true \use_refstyle 0 +\use_minted 0 \index Index \shortcut idx \color #008000 @@ -65,7 +70,10 @@ theorems-ams-bytype \tocdepth 3 \paragraph_separation indent \paragraph_indentation default -\quotes_language english +\is_math_indent 0 +\math_numbering_side default +\quotes_style english +\dynamic_quotes 0 \papercolumns 1 \papersides 1 \paperpagestyle default @@ -98,6 +106,11 @@ width "100col%" special "none" height "1in" height_special "totalheight" +thickness "0.4pt" +separation "3pt" +shadowsize "4pt" +framecolor "black" +backgroundcolor "none" status collapsed \begin_layout Plain Layout @@ -654,6 +667,7 @@ reference "eq:LocalBehavior" \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -934,6 +948,7 @@ See \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -1025,6 +1040,7 @@ See \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -2209,6 +2225,7 @@ instantaneous velocity LatexCommand cite after "page 51 for rotations, page 419 for SE(3)" key "Murray94book" +literal "true" \end_inset @@ -2965,7 +2982,7 @@ B^{T} & I_{3}\end{array}\right] \begin_layout Subsection \begin_inset CommandInset label LatexCommand label -name "sub:Pushforward-of-Between" +name "subsec:Pushforward-of-Between" \end_inset @@ -3419,6 +3436,7 @@ A retraction \begin_inset CommandInset citation LatexCommand cite key "Absil07book" +literal "true" \end_inset @@ -3873,7 +3891,7 @@ BetweenFactor , derived in Section \begin_inset CommandInset ref LatexCommand ref -reference "sub:Pushforward-of-Between" +reference "subsec:Pushforward-of-Between" \end_inset @@ -4430,7 +4448,7 @@ In the case of \begin_inset Formula $\SOthree$ \end_inset - the vector space is + the vector space is \begin_inset Formula $\Rthree$ \end_inset @@ -4502,7 +4520,7 @@ reference "Th:InverseAction" \begin_layout Subsection \begin_inset CommandInset label LatexCommand label -name "sub:3DAngularVelocities" +name "subsec:3DAngularVelocities" \end_inset @@ -4695,6 +4713,7 @@ Absil LatexCommand cite after "page 58" key "Absil07book" +literal "true" \end_inset @@ -5395,6 +5414,7 @@ While not a Lie group, we can define an exponential map, which is given \begin_inset CommandInset citation LatexCommand cite key "Ma01ijcv" +literal "true" \end_inset @@ -5402,6 +5422,7 @@ key "Ma01ijcv" \begin_inset CommandInset href LatexCommand href name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf" +literal "false" \end_inset @@ -5605,6 +5626,7 @@ The exponential map uses \begin_inset CommandInset citation LatexCommand cite key "Absil07book" +literal "true" \end_inset @@ -6293,7 +6315,7 @@ d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right) \end_layout \begin_layout Section -Line3 (Ocaml) +Line3 \end_layout \begin_layout Standard @@ -6345,6 +6367,14 @@ R'=R(I+\Omega) \end_inset + +\end_layout + +\begin_layout Subsection +Projecting Line3 +\end_layout + +\begin_layout Standard Projecting a line to 2D can be done easily, as both \begin_inset Formula $v$ \end_inset @@ -6430,13 +6460,21 @@ or the \end_layout +\begin_layout Subsection +Action of +\begin_inset Formula $\SEthree$ +\end_inset + + on the line +\end_layout + \begin_layout Standard Transforming a 3D line \begin_inset Formula $(R,(a,b))$ \end_inset from a world coordinate frame to a camera frame -\begin_inset Formula $(R_{w}^{c},t^{w})$ +\begin_inset Formula $T_{c}^{w}=(R_{c}^{w},t^{w})$ \end_inset is done by @@ -6466,17 +6504,115 @@ b'=b-R_{2}^{T}t^{w} \end_inset -Again, we need to redo the derivatives, as R is incremented from the right. - The first argument is incremented from the left, but the result is incremented - on the right: +where +\begin_inset Formula $R_{1}$ +\end_inset + + and +\begin_inset Formula $R_{2}$ +\end_inset + + are the columns of +\begin_inset Formula $R$ +\end_inset + + , as before. + +\end_layout + +\begin_layout Standard +To find the derivatives, the transformation of a line +\begin_inset Formula $l^{w}=(R,a,b)$ +\end_inset + + from world coordinates to a camera coordinate frame +\begin_inset Formula $T_{c}^{w}$ +\end_inset + +, specified in world coordinates, can be written as a function +\begin_inset Formula $f:\SEthree\times L\rightarrow L$ +\end_inset + +, as given above, i.e., \begin_inset Formula -\begin{eqnarray*} -R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\ -I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\ -\Omega' & = & R'^{T}\Skew{S\omega}R'\\ -\Omega' & = & \Skew{R'^{T}S\omega}\\ -\omega' & = & R'^{T}S\omega -\end{eqnarray*} +\[ +f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right). +\] + +\end_inset + +Let us find the Jacobian +\begin_inset Formula $J_{1}$ +\end_inset + + of +\begin_inset Formula $f$ +\end_inset + + with respect to the first argument +\begin_inset Formula $T_{c}^{w}$ +\end_inset + +, which should obey +\begin_inset Formula +\begin{align*} +f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi +\end{align*} + +\end_inset + +Note that +\begin_inset Formula +\[ +T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc} +R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +Let's write this out separately for each of +\begin_inset Formula $R,a,b$ +\end_inset + +: +\begin_inset Formula +\begin{align*} +\left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\ +a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\ +b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v +\end{align*} + +\end_inset + +Simplifying, we get: +\begin_inset Formula +\begin{align*} +-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\ +-R_{1}^{T}R_{c}^{w} & \approx J_{av}\\ +-R_{2}^{T}R_{c}^{w} & \approx J_{bv} +\end{align*} + +\end_inset + +which gives the expressions for +\begin_inset Formula $J_{av}$ +\end_inset + + and +\begin_inset Formula $J_{bv}$ +\end_inset + +. + The top line can be further simplified: +\begin_inset Formula +\begin{align*} +-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\ +-R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\ +-\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\ +-R'^{T} & \approx J_{R\omega} +\end{align*} \end_inset @@ -6687,6 +6823,7 @@ Spivak \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -6795,6 +6932,7 @@ The following is adapted from Appendix A in \begin_inset CommandInset citation LatexCommand cite key "Murray94book" +literal "true" \end_inset @@ -6924,6 +7062,7 @@ might be LatexCommand cite after "page 45" key "Hall00book" +literal "true" \end_inset diff --git a/doc/math.pdf b/doc/math.pdf index ab8f1f69a2..8dc7270f11 100644 Binary files a/doc/math.pdf and b/doc/math.pdf differ diff --git a/doc/robust.pdf b/doc/robust.pdf new file mode 100644 index 0000000000..67b853f44e 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 0000000000..0c136f94c8 --- /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 33aa1ab966..0000000000 --- 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 0000000000..9f6eea3b80 --- /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 0000000000..2dac4c3db3 --- /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 0000000000..61ecd9b9ad --- /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 0000000000..21356138f6 --- /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 0000000000..8d280252f6 --- /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 0000000000..c0ab692c68 --- /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 0000000000..c733ceb19d --- /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 0000000000..1696f6c612 --- /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 0000000000..187c76314a --- /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 0000000000..bf545e9c2b --- /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 7251c2b6fe..476f4ae215 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/CameraResectioning.cpp b/examples/CameraResectioning.cpp index e3408b67bf..b124180983 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -18,7 +18,8 @@ #include #include -#include +#include +#include #include using namespace gtsam; @@ -47,7 +48,7 @@ class ResectioningFactor: public NoiseModelFactor1 { /// evaluate the error virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { - SimpleCamera camera(pose, *K_); + PinholeCamera camera(pose, *K_); return camera.project(P_, H, boost::none, boost::none) - p_; } }; diff --git a/examples/Data/Klaus3.g2o b/examples/Data/Klaus3.g2o new file mode 100644 index 0000000000..4c7233fa7d --- /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/pose3Localizationexample.txt b/examples/Data/pose3Localizationexample.txt new file mode 100644 index 0000000000..a35005aa2b --- /dev/null +++ b/examples/Data/pose3Localizationexample.txt @@ -0,0 +1,9 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 +VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 +VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446 +VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488 +EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 diff --git a/examples/Data/toyExample.g2o b/examples/Data/toyExample.g2o new file mode 100755 index 0000000000..5ff1ba74a0 --- /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 0000000000..5dca116c35 --- /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 6eb08c12e9..121df4befe 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; - - - // "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; - - // 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.. - - //Step1: Compute P(S,W) - DiscreteFactorGraph jointFG; - jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices()); - DecisionTreeFactor probSW = jointFG.product(); - - //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); - + 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) = ? + + // add evidence that it is not Cloudy + graph.add(Cloudy, "1 0"); + + // solve again, now with evidence + DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); + DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize(); + + cout << "\nMPE given C=0:" << endl; + print(mpe_with_evidence); + + // 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/FisheyeExample.cpp b/examples/FisheyeExample.cpp new file mode 100644 index 0000000000..2231492993 --- /dev/null +++ b/examples/FisheyeExample.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * 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 FisheyeExample.cpp + * @brief A visualSLAM example for the structure-from-motion problem on a + * simulated dataset. This version uses a fisheye camera model and a GaussNewton + * solver to solve the graph in one batch + * @author ghaggin + * @Date Apr 9,2020 + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// For loading the data +#include "SFMdata.h" + +// Camera observations of landmarks will be stored as Point2 (x, y). +#include + +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols +#include + +// Use GaussNewtonOptimizer to solve graph +#include +#include +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common +// factors have been provided with the library for solving robotics/SLAM/Bundle +// Adjustment problems. Here we will use Projection factors to model the +// camera's landmark observations. Also, we will initialize the robot at some +// location using a Prior factor. +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +using symbol_shorthand::L; // for landmarks +using symbol_shorthand::X; // for poses + +/* ************************************************************************* */ +int main(int argc, char *argv[]) { + // Define the camera calibration parameters + auto K = boost::make_shared( + 278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, + 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625); + + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + + // Create the set of ground-truth landmarks + const vector points = createPoints(); + + // Create the set of ground-truth poses + const vector poses = createPoses(); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z + auto posePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); + graph.emplace_shared>(X(0), poses[0], posePrior); + + // Add a prior on landmark l0 + auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(L(0), points[0], pointPrior); + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static const Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(L(j), points[j] + kDeltaPoint); + + // Loop over the poses, adding the observations to the graph + for (size_t i = 0; i < poses.size(); ++i) { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) { + PinholeCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, X(i), L(j), K); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(X(i), poses[i] * kDeltaPose); + } + + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + params.maxIterations = 10000; + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" << graph.error(initialEstimate) << std::endl; + std::cout << "final error=" << graph.error(result) << std::endl; + + std::ofstream os("examples/vio_batch.dot"); + graph.saveGraph(os, result); + + return 0; +} +/* ************************************************************************* */ diff --git a/examples/HMMExample.cpp b/examples/HMMExample.cpp new file mode 100644 index 0000000000..ee861e3811 --- /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 0000000000..f1d89b47a7 --- /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/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index d8fee15164..7dbd8323bd 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -4,7 +4,8 @@ * @author Alexander (pumaking on BitBucket) */ -#include +#include +#include #include #include #include @@ -56,7 +57,7 @@ int main(int argc, char* argv[]) { vector poses = {pose1, pose2, pose3, pose4, pose5}; // Add first pose - graph.emplace_shared>(X(0), poses[0], noise); + graph.addPrior(X(0), poses[0], noise); initialEstimate.insert(X(0), poses[0].compose(delta)); // Create smart factor with measurement from first pose only @@ -70,7 +71,7 @@ int main(int argc, char* argv[]) { cout << "i = " << i << endl; // Add prior on new pose - graph.emplace_shared>(X(i), poses[i], noise); + graph.addPrior(X(i), poses[i], noise); initialEstimate.insert(X(i), poses[i].compose(delta)); // "Simulate" measurement from this pose diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp index f39e9f4ebf..c0a102d111 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -129,18 +129,16 @@ int main(int argc, char* argv[]) { // Pose prior - at identity auto priorPoseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); - graph.emplace_shared>(X(1), Pose3::identity(), - priorPoseNoise); + graph.addPrior(X(1), Pose3::identity(), priorPoseNoise); initialEstimate.insert(X(0), Pose3::identity()); // Bias prior - graph.add(PriorFactor(B(1), imu.priorImuBias, - imu.biasNoiseModel)); + graph.addPrior(B(1), imu.priorImuBias, + imu.biasNoiseModel); initialEstimate.insert(B(0), imu.priorImuBias); // Velocity prior - assume stationary - graph.add( - PriorFactor(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel)); + graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel); initialEstimate.insert(V(0), Vector3(0, 0, 0)); int lastFrame = 1; diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 25a6adf51e..cb36504219 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,12 +1,12 @@ -#include +#include +#include #include #include #include #include #include #include -#include #include @@ -34,7 +34,7 @@ int main(int argc, char* argv[]) { double radius = 30; const Point3 up(0, 0, 1), target(0, 0, 0); const Point3 position(radius, 0, 0); - const SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + const auto camera = PinholeCamera::Lookat(position, target, up); const auto pose_0 = camera.pose(); // Now, create a constant-twist scenario that makes the camera orbit the @@ -60,21 +60,18 @@ int main(int argc, char* argv[]) { // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z. auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - newgraph.push_back(PriorFactor(X(0), pose_0, noise)); + newgraph.addPrior(X(0), pose_0, noise); // Add imu priors Key biasKey = Symbol('b', 0); auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); - PriorFactor biasprior(biasKey, imuBias::ConstantBias(), - biasnoise); - newgraph.push_back(biasprior); + newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise); initialEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); Vector n_velocity(3); n_velocity << 0, angular_velocity * radius, 0; - PriorFactor velprior(V(0), n_velocity, velnoise); - newgraph.push_back(velprior); + newgraph.addPrior(V(0), n_velocity, velnoise); initialEstimate.insert(V(0), n_velocity); diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index e038f51176..63355631b5 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -11,12 +11,14 @@ /** * @file imuFactorsExample - * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code. + * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor + * navigation code. * @author Garrett (ghemann@gmail.com), Luca Carlone */ /** - * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS + * Example of use of the imuFactors (imuFactor and combinedImuFactor) in + * conjunction with GPS * - imuFactor is used by default. You can test combinedImuFactor by * appending a `-c` flag at the end (see below for example command). * - we read IMU and GPS data from a CSV file, with the following format: @@ -26,52 +28,64 @@ * linAccN, linAccE, linAccD, angVelN, angVelE, angVelD * A row starting with "1" is a gps correction formatted with * N, E, D, qX, qY, qZ, qW - * Note that for GPS correction, we're only using the position not the rotation. The - * rotation is provided in the file for ground truth comparison. + * Note that for GPS correction, we're only using the position not the + * rotation. The rotation is provided in the file for ground truth comparison. * * Usage: ./ImuFactorsExample [data_csv_path] [-c] * 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 +// Uncomment the following to use Levenberg Marquardt Optimizer +// #define USE_LM + using namespace gtsam; using namespace std; -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) - -const string output_filename = "imuFactorExampleResults.csv"; -const string use_combined_imu_flag = "-c"; +using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -// This will either be PreintegratedImuMeasurements (for ImuFactor) or -// PreintegratedCombinedMeasurements (for CombinedImuFactor). -PreintegrationType *imu_preintegrated_; +static const char output_filename[] = "imuFactorExampleResults.csv"; +static const char use_combined_imu_flag[3] = "-c"; -int main(int argc, char* argv[]) -{ +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"); - } else if (argc < 3){ - if (strcmp(argv[1], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match + } else if (argc < 3) { + if (strcmp(argv[1], use_combined_imu_flag) == 0) { printf("using CombinedImuFactor\n"); use_combined_imu = true; printf("using default CSV file\n"); @@ -81,15 +95,17 @@ int main(int argc, char* argv[]) } } else { data_filename = argv[1]; - if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match + if (strcmp(argv[2], use_combined_imu_flag) == 0) { printf("using CombinedImuFactor\n"); use_combined_imu = true; } } // Set up output file for plotting errors - 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),gt_qx,gt_qy,gt_qz,gt_qw\n"); + FILE* fp_out = fopen(output_filename, "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),gt_qx," + "gt_qy,gt_qz,gt_qw\n"); // Begin parsing the CSV file. Input the first line for initialization. // From there, we'll iterate through the file and we'll preintegrate the IMU @@ -98,9 +114,9 @@ int main(int argc, char* argv[]) string value; // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) - Eigen::Matrix initial_state = Eigen::Matrix::Zero(); - getline(file, value, ','); // i - for (int i=0; i<9; i++) { + Vector10 initial_state; + getline(file, value, ','); // i + for (int i = 0; i < 9; i++) { getline(file, value, ','); initial_state(i) = atof(value.c_str()); } @@ -108,13 +124,14 @@ int main(int argc, char* argv[]) initial_state(9) = atof(value.c_str()); cout << "initial state:\n" << initial_state.transpose() << "\n\n"; - // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z); + // Assemble initial quaternion through GTSAM constructor + // ::quaternion(w,x,y,z); Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), initial_state(4), initial_state(5)); Point3 prior_point(initial_state.head<3>()); Pose3 prior_pose(prior_rotation, prior_point); Vector3 prior_velocity(initial_state.tail<3>()); - imuBias::ConstantBias prior_imu_bias; // assume zero initial bias + imuBias::ConstantBias prior_imu_bias; // assume zero initial bias Values initial_values; int correction_count = 0; @@ -122,57 +139,64 @@ int main(int argc, char* argv[]) initial_values.insert(V(correction_count), prior_velocity); initial_values.insert(B(correction_count), prior_imu_bias); - // Assemble prior noise model and add it the graph. - noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m - noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s - noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3); + // Assemble prior noise model and add it the graph.` + auto pose_noise_model = noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5) + .finished()); // rad,rad,rad,m, m, m + auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s + auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3); // Add all prior factors (pose, velocity, bias) to the graph. - NonlinearFactorGraph *graph = new NonlinearFactorGraph(); - graph->add(PriorFactor(X(correction_count), prior_pose, pose_noise_model)); - graph->add(PriorFactor(V(correction_count), prior_velocity,velocity_noise_model)); - graph->add(PriorFactor(B(correction_count), prior_imu_bias,bias_noise_model)); + NonlinearFactorGraph* graph = new NonlinearFactorGraph(); + graph->addPrior(X(correction_count), prior_pose, pose_noise_model); + graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model); + graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model); // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; double gyro_noise_sigma = 0.000205689024915; double accel_bias_rw_sigma = 0.004905; double gyro_bias_rw_sigma = 0.000001454441043; - Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2); - Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2); - Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities - Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2); - Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2); - Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration - - boost::shared_ptr p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration + + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); // PreintegrationBase params: - p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous - p->integrationCovariance = integration_error_cov; // integration uncertainty continuous + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous // should be using 2nd order integration // PreintegratedRotation params: - p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous // PreintegrationCombinedMeasurements params: - p->biasAccCovariance = bias_acc_cov; // acc bias in continuous - p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous p->biasAccOmegaInt = bias_acc_omega_int; - std::shared_ptr imu_preintegrated_ = nullptr; + std::shared_ptr preintegrated = nullptr; if (use_combined_imu) { - imu_preintegrated_ = + preintegrated = std::make_shared(p, prior_imu_bias); } else { - imu_preintegrated_ = + preintegrated = std::make_shared(p, prior_imu_bias); } - assert(imu_preintegrated_); + assert(preintegrated); - // Store previous state for the imu integration and the latest predicted outcome. + // Store previous state for imu integration and latest predicted outcome. NavState prev_state(prior_pose, prior_velocity); NavState prop_state = prev_state; imuBias::ConstantBias prev_bias = prior_imu_bias; - // Keep track of the total error over the entire run for a simple performance metric. + // Keep track of total error over the entire run as simple performance metric. double current_position_error = 0.0, current_orientation_error = 0.0; double output_time = 0.0; @@ -181,14 +205,13 @@ int main(int argc, char* argv[]) // All priors have been set up, now iterate through the data file. while (file.good()) { - // Parse out first value getline(file, value, ','); int type = atoi(value.c_str()); - if (type == 0) { // IMU measurement - Eigen::Matrix imu = Eigen::Matrix::Zero(); - for (int i=0; i<5; ++i) { + if (type == 0) { // IMU measurement + Vector6 imu; + for (int i = 0; i < 5; ++i) { getline(file, value, ','); imu(i) = atof(value.c_str()); } @@ -196,11 +219,11 @@ int main(int argc, char* argv[]) imu(5) = atof(value.c_str()); // Adding the IMU preintegration. - imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); + preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); - } else if (type == 1) { // GPS measurement - Eigen::Matrix gps = Eigen::Matrix::Zero(); - for (int i=0; i<6; ++i) { + } else if (type == 1) { // GPS measurement + Vector7 gps; + for (int i = 0; i < 6; ++i) { getline(file, value, ','); gps(i) = atof(value.c_str()); } @@ -211,53 +234,61 @@ int main(int argc, char* argv[]) // Adding IMU factor and GPS factor and optimizing. if (use_combined_imu) { - const PreintegratedCombinedMeasurements& preint_imu_combined = + auto preint_imu_combined = dynamic_cast( - *imu_preintegrated_); - CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1), - X(correction_count ), V(correction_count ), - B(correction_count-1), B(correction_count ), - preint_imu_combined); + *preintegrated); + CombinedImuFactor imu_factor( + X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), B(correction_count - 1), + B(correction_count), preint_imu_combined); graph->add(imu_factor); } else { - const PreintegratedImuMeasurements& preint_imu = - dynamic_cast( - *imu_preintegrated_); - ImuFactor imu_factor(X(correction_count-1), V(correction_count-1), - X(correction_count ), V(correction_count ), - B(correction_count-1), - preint_imu); + auto preint_imu = + dynamic_cast(*preintegrated); + ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), + B(correction_count - 1), preint_imu); graph->add(imu_factor); imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor(B(correction_count-1), - B(correction_count ), - zero_bias, bias_noise_model)); + graph->add(BetweenFactor( + B(correction_count - 1), B(correction_count), zero_bias, + bias_noise_model)); } - noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0); + auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); GPSFactor gps_factor(X(correction_count), - Point3(gps(0), // N, - gps(1), // E, - gps(2)), // D, + Point3(gps(0), // N, + gps(1), // E, + gps(2)), // D, correction_noise); graph->add(gps_factor); // Now optimize and compare results. - prop_state = imu_preintegrated_->predict(prev_state, prev_bias); + prop_state = preintegrated->predict(prev_state, prev_bias); initial_values.insert(X(correction_count), prop_state.pose()); 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))); prev_bias = result.at(B(correction_count)); // Reset the preintegration object. - imu_preintegrated_->resetIntegrationAndSetBias(prev_bias); + preintegrated->resetIntegrationAndSetBias(prev_bias); // Print out the position and orientation error for comparison. Vector3 gtsam_position = prev_state.pose().translation(); @@ -268,19 +299,19 @@ int main(int argc, char* argv[]) Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5)); Quaternion quat_error = gtsam_quat * gps_quat.inverse(); quat_error.normalize(); - Vector3 euler_angle_error(quat_error.x()*2, - quat_error.y()*2, - quat_error.z()*2); + Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2, + quat_error.z() * 2); current_orientation_error = euler_angle_error.norm(); // display statistics - cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n"; + cout << "Position error:" << current_position_error << "\t " + << "Angular error:" << current_orientation_error << "\n"; fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", - output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2), - gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(), - gps(0), gps(1), gps(2), - gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); + output_time, gtsam_position(0), gtsam_position(1), + gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), + gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(), + gps_quat.y(), gps_quat.z(), gps_quat.w()); output_time += 1.0; @@ -290,6 +321,7 @@ int main(int argc, char* argv[]) } } fclose(fp_out); - cout << "Complete, results written to " << output_filename << "\n\n";; + cout << "Complete, results written to " << output_filename << "\n\n"; + return 0; } diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp index 9e86886e72..1df83d9a1a 100644 --- a/examples/InverseKinematicsExampleExpressions.cpp +++ b/examples/InverseKinematicsExampleExpressions.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 3c07fd4e3a..a28ead5fe5 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -66,12 +66,11 @@ using namespace gtsam; #include class UnaryFactor: public NoiseModelFactor1 { - // The factor will hold a measurement consisting of an (X,Y) location // We could this with a Point2 but here we just use two doubles double mx_, my_; -public: + public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -85,15 +84,15 @@ class UnaryFactor: public NoiseModelFactor1 { // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. - Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const - { + Vector evaluateError(const Pose2& q, + boost::optional H = boost::none) const { // The measurement function for a GPS-like measurement is simple: // error_x = pose.x - measurement.x // error_y = pose.y - measurement.y // Consequently, the Jacobians are: // [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0] // [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0] - if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished(); + if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); } @@ -107,29 +106,28 @@ class UnaryFactor: public NoiseModelFactor1 { // Additionally, we encourage you the use of unit testing your custom factors, // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below. - -}; // UnaryFactor +}; // UnaryFactor int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it NonlinearFactorGraph graph; // 2a. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. - noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y + auto unaryNoise = + noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y graph.emplace_shared(1, 0.0, 0.0, unaryNoise); graph.emplace_shared(2, 2.0, 0.0, unaryNoise); graph.emplace_shared(3, 4.0, 0.0, unaryNoise); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values @@ -137,7 +135,7 @@ int main(int argc, char** argv) { initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize using Levenberg-Marquardt optimization. The optimizer // accepts an optional set of configuration parameters, controlling diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 76b5098f64..b91e113a45 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -11,7 +11,8 @@ /** * @file METISOrdering.cpp - * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering + * @brief Simple robot motion example, with prior and two odometry measurements, + * using a METIS ordering * @author Frank Dellaert * @author Andrew Melim */ @@ -22,12 +23,11 @@ */ #include -#include -#include -#include #include #include +#include #include +#include using namespace std; using namespace gtsam; @@ -35,26 +35,26 @@ using namespace gtsam; int main(int argc, char** argv) { NonlinearFactorGraph graph; - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, priorMean, priorNoise); + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.addPrior(1, priorMean, priorNoise); Pose2 odometry(2.0, 0.0, 0.0); - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.emplace_shared >(1, 2, odometry, odometryNoise); graph.emplace_shared >(2, 3, odometry, odometryNoise); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print Values initial; initial.insert(1, Pose2(0.5, 0.0, 0.2)); initial.insert(2, Pose2(2.3, 0.1, -0.2)); initial.insert(3, Pose2(4.1, 0.1, 0.1)); - initial.print("\nInitial Estimate:\n"); // print + initial.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization LevenbergMarquardtParams params; - // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" - // By default this parameter is set to OrderingType::COLAMD + // In order to specify the ordering type, we need to se the "orderingType". By + // default this parameter is set to OrderingType::COLAMD params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 6dc0d9a931..1944d9e463 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -29,7 +29,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -57,24 +56,23 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // Create an empty nonlinear factor graph NonlinearFactorGraph graph; // Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, priorMean, priorNoise); + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.addPrior(1, priorMean, priorNoise); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.emplace_shared >(1, 2, odometry, odometryNoise); graph.emplace_shared >(2, 3, odometry, odometryNoise); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values @@ -82,7 +80,7 @@ int main(int argc, char** argv) { initial.insert(1, Pose2(0.5, 0.0, 0.2)); initial.insert(2, Pose2(2.3, 0.1, -0.2)); initial.insert(3, Pose2(4.1, 0.1, 0.1)); - initial.print("\nInitial Estimate:\n"); // print + initial.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 52638fdcaa..ede27131db 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -40,7 +40,6 @@ // Here we will use a RangeBearing factor for the range-bearing measurements to identified // landmarks, and Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include #include @@ -70,35 +69,36 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // Create a factor graph NonlinearFactorGraph graph; // Create the keys we need for this simple example - static Symbol x1('x',1), x2('x',2), x3('x',3); - static Symbol l1('l',1), l2('l',2); + static Symbol x1('x', 1), x2('x', 2), x3('x', 3); + static Symbol l1('l', 1), l2('l', 2); - // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) - Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph + // Add a prior on pose x1 at the origin. A prior factor consists of a mean and + // a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin + auto priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.addPrior(x1, prior, priorNoise); // add directly to graph // Add two odometry factors - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + Pose2 odometry(2.0, 0.0, 0.0); + // create a measurement for both factors (the same in this case) + auto odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta graph.emplace_shared >(x1, x2, odometry, odometryNoise); graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + auto measurementNoise = noiseModel::Diagonal::Sigmas( + Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range // create the measurement values - indices are (pose id, landmark id) - Rot2 bearing11 = Rot2::fromDegrees(45), - bearing21 = Rot2::fromDegrees(90), + Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), bearing32 = Rot2::fromDegrees(90); - double range11 = std::sqrt(4.0+4.0), - range21 = 2.0, - range32 = 2.0; + double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0; // Add Bearing-Range factors graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); @@ -111,7 +111,7 @@ int main(int argc, char** argv) { // Create (deliberately inaccurate) initial estimate Values initialEstimate; initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2)); initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(l1, Point2(1.8, 2.1)); initialEstimate.insert(l2, Point2(4.1, 1.8)); @@ -139,4 +139,3 @@ int main(int argc, char** argv) { return 0; } - diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index f977a08afd..f3d5559303 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -36,7 +36,6 @@ // Here we will use Between factors for the relative motion described by odometry measurements. // We will also use a Between Factor to encode the loop closure constraint // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -65,21 +64,20 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.addPrior(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0), model); graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); @@ -89,17 +87,17 @@ int main(int argc, char** argv) { // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. We will use another Between Factor to enforce this constraint: graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values Values initialEstimate; - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); - initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI)); initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer // The optimizer accepts an optional set of configuration parameters, diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 1f6de6cb11..41037d012e 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -21,7 +21,6 @@ #include // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -31,7 +30,6 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it ExpressionFactorGraph graph; @@ -39,31 +37,32 @@ int main(int argc, char** argv) { Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise); - // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + // For simplicity, we use the same noise model for odometry and loop closures + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model); - graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model); - graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model); - graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model); + graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model); - graph.print("\nFactor Graph:\n"); // print + graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model); + graph.print("\nFactor Graph:\n"); // print - // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values + // 3. Create the data structure to hold the initialEstimate estimate to the + // solution For illustrative purposes, these have been deliberately set to + // incorrect values Values initialEstimate; - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); - initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI)); initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer GaussNewtonParams parameters; diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 35f9884e13..fb3869a507 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -19,7 +19,6 @@ */ #include -#include #include #include #include @@ -29,60 +28,60 @@ using namespace gtsam; // HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber) int main(const int argc, const char *argv[]) { - string kernelType = "none"; - int maxIterations = 100; // default - string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default + int maxIterations = 100; // default + string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default // Parse user's inputs - if (argc > 1){ - g2oFile = argv[1]; // input dataset filename - // outputFile = g2oFile = argv[2]; // done later + if (argc > 1) { + g2oFile = argv[1]; // input dataset filename } - if (argc > 3){ - maxIterations = atoi(argv[3]); // user can specify either tukey or huber + if (argc > 3) { + maxIterations = atoi(argv[3]); // user can specify either tukey or huber } - if (argc > 4){ - kernelType = argv[4]; // user can specify either tukey or huber + if (argc > 4) { + kernelType = argv[4]; // user can specify either tukey or huber } // reading file and creating factor graph NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; bool is3D = false; - if(kernelType.compare("none") == 0){ - boost::tie(graph, initial) = readG2o(g2oFile,is3D); + if (kernelType.compare("none") == 0) { + boost::tie(graph, initial) = readG2o(g2oFile, is3D); } - if(kernelType.compare("huber") == 0){ + if (kernelType.compare("huber") == 0) { std::cout << "Using robust kernel: huber " << std::endl; - boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER); + boost::tie(graph, initial) = + readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); } - if(kernelType.compare("tukey") == 0){ + if (kernelType.compare("tukey") == 0) { std::cout << "Using robust kernel: tukey " << std::endl; - boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY); + boost::tie(graph, initial) = + readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); } // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = *graph; - noiseModel::Diagonal::shared_ptr priorModel = // + auto priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graph->addPrior(0, Pose2(), priorModel); std::cout << "Adding prior on pose 0 " << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); if (argc > 3) { params.maxIterations = maxIterations; - std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl; + std::cout << "User required to perform maximum " << params.maxIterations + << " iterations " << std::endl; } std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "final error=" <error(result)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "final error=" << graph->error(result) << std::endl; if (argc < 3) { result.print("result"); diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index c3d901507a..c4301d28de 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -17,7 +17,6 @@ */ // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -43,7 +42,7 @@ int main (int argc, char** argv) { // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); - graph->push_back(PriorFactor(0, priorMean, priorNoise)); + graph -> addPrior(0, priorMean, priorNoise); // Single Step Optimization using Levenberg-Marquardt Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 99711da2d2..27d5567252 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -17,7 +17,6 @@ */ // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -26,20 +25,20 @@ using namespace std; using namespace gtsam; -int main (int argc, char** argv) { - +int main(int argc, char** argv) { // 1. Create a factor graph container and add factors to it NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.addPrior(1, Pose2(0, 0, 0), priorNoise); - // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + // For simplicity, we will use the same noise model for odometry and loop + // closures + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0), model); graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); @@ -50,10 +49,10 @@ int main (int argc, char** argv) { // 3. Create the data structure to hold the initial estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values Values initial; - initial.insert(1, Pose2(0.5, 0.0, 0.2 )); - initial.insert(2, Pose2(2.3, 0.1, -0.2 )); - initial.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initial.insert(4, Pose2(4.0, 2.0, M_PI )); + initial.insert(1, Pose2(0.5, 0.0, 0.2)); + initial.insert(2, Pose2(2.3, 0.1, -0.2)); + initial.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initial.insert(4, Pose2(4.0, 2.0, M_PI)); initial.insert(5, Pose2(2.1, 2.1, -M_PI_2)); // Single Step Optimization using Levenberg-Marquardt diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index b83dfa1dba..1d02c64fa3 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #include @@ -29,7 +28,6 @@ using namespace std; using namespace gtsam; int main(const int argc, const char *argv[]) { - // Read graph from file string g2oFile; if (argc < 2) @@ -42,14 +40,12 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = *graph; - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - graphWithPrior.print(); + auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); + graph->addPrior(0, Pose2(), priorModel); + graph->print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = lago::initialize(graphWithPrior); + Values estimateLago = lago::initialize(*graph); std::cout << "done!" << std::endl; if (argc < 3) { @@ -57,7 +53,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, estimateLago, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, estimateLago, outputFile); std::cout << "done! " << std::endl; } diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp index 0f306b7f40..d7ab55aaa4 100644 --- a/examples/Pose2SLAMStressTest.cpp +++ b/examples/Pose2SLAMStressTest.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -48,7 +47,7 @@ void testGtsam(int numberNodes) { Pose3 first = Pose3(first_M); NonlinearFactorGraph graph; - graph.add(PriorFactor(0, first, priorModel)); + graph.addPrior(0, first, priorModel); // vo noise model auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 9b459d7b83..cd74b2c071 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -17,7 +17,6 @@ */ // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -29,45 +28,45 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - Pose2 prior(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, prior, priorNoise); + Pose2 prior(0.0, 0.0, 0.0); // prior at origin + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + graph.addPrior(1, prior, priorNoise); // 2b. Add odometry factors - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.emplace_shared >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.emplace_shared >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add the loop closure constraint - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); - graph.print("\nFactor Graph:\n"); // print - + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), + model); + graph.print("\nFactor Graph:\n"); // print - // 3. Create the data structure to hold the initialEstimate estimate to the solution + // 3. Create the data structure to hold the initialEstimate estimate to the + // solution Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8)); initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2)); - initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8)); + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Single Step Optimization using Levenberg-Marquardt LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - // LM is still the outer optimization loop, but by specifying "Iterative" below - // We indicate that an iterative linear solver should be used. - // In addition, the *type* of the iterativeParams decides on the type of + // LM is still the outer optimization loop, but by specifying "Iterative" + // below We indicate that an iterative linear solver should be used. In + // addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) parameters.linearSolverType = NonlinearOptimizerParams::Iterative; parameters.iterativeParams = boost::make_shared(); diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp new file mode 100644 index 0000000000..1667b43d9d --- /dev/null +++ b/examples/Pose3Localization.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 Pose3Localization.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3Localization input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char* argv[]) { + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3Localizationexample.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + // Add prior on the first key + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + Key firstKey = 0; + for (const Values::ConstKeyValuePair& key_value : *initial) { + std::cout << "Adding prior to g2o file " << std::endl; + firstKey = key_value.key; + graph->addPrior(firstKey, Pose3(), priorModel); + break; + } + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); // show info about stopping conditions + GaussNewtonOptimizer optimizer(*graph, *initial, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "final error=" << graph->error(result) << std::endl; + + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); + std::cout << "done! " << std::endl; + } + + // Calculate and print marginal covariances for all variables + Marginals marginals(*graph, result); + for (const auto& key_value : result) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + std::cout << marginals.marginalCovariance(key_value.key) << endl; + } + return 0; +} diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index cceaac4ee5..abadce975e 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -19,7 +19,6 @@ #include #include -#include #include using namespace std; diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 5066222e59..1cca92f591 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -10,24 +10,22 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3SLAMExample_g2o.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ #include #include -#include #include #include using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -41,33 +39,35 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior(firstKey, Pose3(), priorModel); break; } std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; - params.setVerbosity("TERMINATION"); // this will show info about stopping conditions - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + params.setVerbosity("TERMINATION"); // show info about stopping conditions + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "final error=" <error(result)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "final error=" << graph->error(result) << std::endl; if (argc < 3) { result.print("result"); } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index e90d014c06..00a546bb2f 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3SLAMExample_initializePose3Chordal.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -20,14 +20,12 @@ #include #include #include -#include #include using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -41,27 +39,29 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior(firstKey, Pose3(), priorModel); break; } std::cout << "Initializing Pose3 - chordal relaxation" << std::endl; - Values initialization = InitializePose3::initialize(graphWithPrior); + Values initialization = InitializePose3::initialize(*graph); std::cout << "done!" << std::endl; if (argc < 3) { initialization.print("initialization"); } else { const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, initialization, outputFile); + std::cout << "Writing results to file: " << outputFile << std::endl; + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, initialization, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 10960cf316..2f92afb9ed 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3SLAMExample_initializePose3Gradient.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -20,14 +20,12 @@ #include #include #include -#include #include using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -41,31 +39,35 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior(firstKey, Pose3(), priorModel); break; } std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl; bool useGradient = true; - Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); + Values initialization = + InitializePose3::initialize(*graph, *initial, useGradient); std::cout << "done!" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "initialization error=" <error(initialization)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "initialization error=" << graph->error(initialization) + << std::endl; if (argc < 3) { initialization.print("initialization"); } else { const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, initialization, outputFile); + std::cout << "Writing results to file: " << outputFile << std::endl; + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, initialization, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 7b1667e122..c8e31e1c55 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -37,7 +37,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. -#include #include #include #include @@ -124,7 +123,7 @@ int main (int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(0, pose0, priorNoise)); + newFactors.addPrior(0, pose0, priorNoise); Values initial; initial.insert(0, pose0); diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 8c87fa315a..fddca81694 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -30,7 +30,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Projection factors to model the camera's landmark observations. // Also, we will initialize the robot at some location using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -57,12 +56,12 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Define the camera calibration parameters Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks vector points = createPoints(); @@ -74,32 +73,45 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; // Add a prior on pose x1. This indirectly specifies where the origin is. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph + auto poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) + .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph - // Simulated measurements from each camera pose, adding them to the factor graph + // Simulated measurements from each camera pose, adding them to the factor + // graph for (size_t i = 0; i < poses.size(); ++i) { - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + graph.emplace_shared >( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } } - // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained - // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance - // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph + // Because the structure-from-motion problem has a scale ambiguity, the + // problem is still under-constrained Here we add a prior on the position of + // the first landmark. This fixes the scale by indicating the distance between + // the first camera and the first landmark. All other landmark positions are + // interpreted using this scale. + auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.addPrior(Symbol('l', 0), points[0], + pointNoise); // add directly to graph graph.print("Factor Graph:\n"); // Create the data structure to hold the initial estimate to the solution // Intentionally initialize the variables off from the ground truth Values initialEstimate; - for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); - for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + for (size_t i = 0; i < poses.size(); ++i) { + auto corrupted_pose = poses[i].compose( + Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))); + initialEstimate.insert( + Symbol('x', i), corrupted_pose); + } + for (size_t j = 0; j < points.size(); ++j) { + Point3 corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15); + initialEstimate.insert(Symbol('l', j), corrupted_point); + } initialEstimate.print("Initial Estimates:\n"); /* Optimize the graph and print results */ diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 191664ef60..73b6f27f7e 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -27,7 +27,7 @@ #include // Header order is close to far -#include +#include "SFMdata.h" #include #include #include @@ -67,7 +67,7 @@ int main(int argc, char* argv[]) { // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { Pose3_ x('x', i); - SimpleCamera camera(poses[i], K); + PinholeCamera camera(poses[i], K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); // Below an expression for the prediction of the measurement: diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 22af9492f2..3768ee2a34 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -28,7 +28,7 @@ // Header order is close to far #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! #include using namespace std; @@ -40,20 +40,16 @@ using symbol_shorthand::P; // An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration // and has a total of 9 free parameters -/* ************************************************************************* */ int main(int argc, char* argv[]) { - // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); - if (argc > 1) - filename = string(argv[1]); + if (argc > 1) filename = string(argv[1]); // Load the SfM data from file SfmData mydata; readBAL(filename, mydata); - cout - << boost::format("read %1% tracks on %2% cameras\n") - % mydata.number_tracks() % mydata.number_cameras(); + cout << boost::format("read %1% tracks on %2% cameras\n") % + mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph ExpressionFactorGraph graph; @@ -65,23 +61,23 @@ int main(int argc, char* argv[]) { Pose3_ pose0_(&SfmCamera::getPose, camera0_); // Finally, we say it should be equal to first guess graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(), - noiseModel::Isotropic::Sigma(6, 0.1)); + noiseModel::Isotropic::Sigma(6, 0.1)); // similarly, we create a prior on the first point Point3_ point0_(P(0)); graph.addExpressionFactor(point0_, mydata.tracks[0].p, - noiseModel::Isotropic::Sigma(3, 0.1)); + noiseModel::Isotropic::Sigma(3, 0.1)); // We share *one* noiseModel between all projection factors - noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, - 1.0); // one pixel in u and v + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - // Simulated measurements from each camera pose, adding them to the factor graph + // Simulated measurements from each camera pose, adding them to the factor + // graph size_t j = 0; - for(const SfmTrack& track: mydata.tracks) { + for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for(const SfmMeasurement& m: track.measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera @@ -98,10 +94,8 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0; j = 0; - for(const SfmCamera& camera: mydata.cameras) - initial.insert(C(i++), camera); - for(const SfmTrack& track: mydata.tracks) - initial.insert(P(j++), track.p); + for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera); + for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p); /* Optimize the graph and print results */ Values result; @@ -117,5 +111,3 @@ int main(int argc, char* argv[]) { return 0; } -/* ************************************************************************* */ - diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index c5545fc0c3..86e6b8ae98 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -44,7 +44,7 @@ int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses @@ -80,15 +80,15 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared >(0, poses[0], noise); + graph.addPrior(0, poses[0], noise); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.emplace_shared >(1, poses[1], noise); // add directly to graph + graph.addPrior(1, poses[1], noise); // add directly to graph graph.print("Factor Graph:\n"); @@ -117,7 +117,7 @@ int main(int argc, char* argv[]) { // The output of point() is in boost::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 075e2a6531..3f553efc6f 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -35,13 +35,12 @@ typedef PinholePose Camera; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Define the camera calibration parameters Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = - noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses vector points = createPoints(); @@ -52,17 +51,16 @@ int main(int argc, char* argv[]) { // Simulated measurements from each camera pose, adding them to the factor graph for (size_t j = 0; j < points.size(); ++j) { - - // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + // every landmark represent a single landmark, we use shared pointer to init + // the factor, and then insert measurements. SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { - // generate the 2D measurement Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); - // call add() function to add measurement into a single factor, here we need to add: + // call add() function to add measurement into a single factor smartfactor->add(measurement, i); } @@ -72,12 +70,12 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared >(0, poses[0], noise); + graph.addPrior(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.emplace_shared >(1, poses[0], noise); + graph.addPrior(1, poses[0], noise); // Create the initial estimate to the solution Values initialEstimate; @@ -85,9 +83,9 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(i, poses[i].compose(delta)); - // We will use LM in the outer optimization loop, but by specifying "Iterative" below - // We indicate that an iterative linear solver should be used. - // In addition, the *type* of the iterativeParams decides on the type of + // We will use LM in the outer optimization loop, but by specifying + // "Iterative" below We indicate that an iterative linear solver should be + // used. In addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) LevenbergMarquardtParams parameters; parameters.linearSolverType = NonlinearOptimizerParams::Iterative; @@ -110,11 +108,10 @@ int main(int argc, char* argv[]) { result.print("Final results:\n"); Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - SmartFactor::shared_ptr smart = // - boost::dynamic_pointer_cast(graph[j]); + auto smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 9d5ad0b334..ffb5b195b9 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include // for loading BAL datasets ! #include @@ -50,7 +49,7 @@ int main (int argc, char* argv[]) { NonlinearFactorGraph graph; // We share *one* noiseModel between all projection factors - noiseModel::Isotropic::shared_ptr noise = + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph @@ -66,8 +65,8 @@ int main (int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); - graph.emplace_shared > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); + graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b3dd3d25ef..b79a9fa285 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SFMExample.cpp + * @file SFMExample_bal_COLAMD_METIS.cpp * @brief This file is to compare the ordering performance for COLAMD vs METIS. * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file. * @author Frank Dellaert, Zhaoyang Lv @@ -21,9 +21,8 @@ #include #include #include -#include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! #include @@ -35,50 +34,52 @@ using symbol_shorthand::C; using symbol_shorthand::P; // We will be using a projection factor that ties a SFM_Camera to a 3D point. -// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration -// and has a total of 9 free parameters -typedef GeneralSFMFactor MyFactor; +// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler +// calibration and has a total of 9 free parameters +typedef GeneralSFMFactor MyFactor; -/* ************************************************************************* */ -int main (int argc, char* argv[]) { +int main(int argc, char* argv[]) { // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); - if (argc>1) filename = string(argv[1]); + if (argc > 1) filename = string(argv[1]); // Load the SfM data from file SfmData mydata; readBAL(filename, mydata); - cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + cout << boost::format("read %1% tracks on %2% cameras\n") % + mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph NonlinearFactorGraph graph; // We share *one* noiseModel between all projection factors - noiseModel::Isotropic::shared_ptr noise = - noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph size_t j = 0; - for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.measurements) { + for (const SfmTrack& track : mydata.tracks) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P + graph.emplace_shared( + uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); - graph.emplace_shared >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); + graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.addPrior(P(0), mydata.tracks[0].p, + noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; - size_t i = 0; j = 0; - for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera); - for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p); + size_t i = 0; + j = 0; + for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera); + for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p); /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ @@ -99,7 +100,7 @@ int main (int argc, char* argv[]) { } // expect they have different ordering results - if(params_using_COLAMD.ordering == params_using_METIS.ordering) { + if (params_using_COLAMD.ordering == params_using_METIS.ordering) { cout << "COLAMD and METIS produce the same ordering. " << "Problem here!!!" << endl; } @@ -121,8 +122,7 @@ int main (int argc, char* argv[]) { cout << e.what(); } - - { // printing the result + { // printing the result cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; cout << "METIS final error: " << graph.error(result_METIS) << endl; @@ -130,15 +130,13 @@ int main (int argc, char* argv[]) { cout << endl << endl; cout << "Time comparison by solving " << filename << " results:" << endl; - cout << boost::format("%1% point tracks and %2% cameras\n") \ - % mydata.number_tracks() % mydata.number_cameras() \ + cout << boost::format("%1% point tracks and %2% cameras\n") % + mydata.number_tracks() % mydata.number_cameras() << endl; tictoc_print_(); } - return 0; } -/* ************************************************************************* */ diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 5462868c98..04d3c9e479 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SFMMdata.h + * @file SFMdata.h * @brief Simple example for the structure-from-motion problems * @author Duy-Nguyen Ta */ @@ -30,7 +30,8 @@ #include // We will also need a camera object to hold calibration information and perform projections. -#include +#include +#include /* ************************************************************************* */ std::vector createPoints() { diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index ba097c0744..2018600120 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -33,8 +33,7 @@ #include // SFM-specific factors -#include -#include // does calibration ! +#include // does calibration ! // Standard headers #include @@ -42,9 +41,7 @@ using namespace std; using namespace gtsam; -/* ************************************************************************* */ int main(int argc, char* argv[]) { - // Create the set of ground-truth vector points = createPoints(); vector poses = createPoses(); @@ -53,38 +50,51 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; // Add a prior on pose x1. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); + auto poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) + .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.addPrior(Symbol('x', 0), poses[0], poseNoise); - // Simulated measurements from each camera pose, adding them to the factor graph + // Simulated measurements from each camera pose, adding them to the factor + // graph Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); for (size_t i = 0; i < poses.size(); ++i) { for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], K); + PinholeCamera camera(poses[i], K); Point2 measurement = camera.project(points[j]); - // The only real difference with the Visual SLAM example is that here we use a - // different factor type, that also calculates the Jacobian with respect to calibration - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)); + // The only real difference with the Visual SLAM example is that here we + // use a different factor type, that also calculates the Jacobian with + // respect to calibration + graph.emplace_shared >( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), + Symbol('K', 0)); } } // Add a prior on the position of the first landmark. - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph + auto pointNoise = + noiseModel::Isotropic::Sigma(3, 0.1); + graph.addPrior(Symbol('l', 0), points[0], + pointNoise); // add directly to graph // Add a prior on the calibration. - noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished()); - graph.emplace_shared >(Symbol('K', 0), K, calNoise); + auto calNoise = noiseModel::Diagonal::Sigmas( + (Vector(5) << 500, 500, 0.1, 100, 100).finished()); + graph.addPrior(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters Values initialEstimate; initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.insert( + Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), + points[j] + Point3(-0.25, 0.20, 0.15)); /* Optimize the graph and print results */ Values result = DoglegOptimizer(graph, initialEstimate).optimize(); @@ -92,5 +102,4 @@ int main(int argc, char* argv[]) { return 0; } -/* ************************************************************************* */ diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index f70a44eec2..19909e3538 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -32,8 +32,8 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// We will apply a simple prior on the rotation -#include +// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience +// method in NonlinearFactorGraph. // When the factors are created, we will add them to a Factor Graph. As the factors we are using // are nonlinear factors, we will need a Nonlinear Factor Graph. @@ -59,7 +59,6 @@ using namespace gtsam; const double degree = M_PI / 180; int main() { - /** * Step 1: Create a factor to express a unary constraint * The "prior" in this case is the measurement from a sensor, @@ -76,9 +75,8 @@ int main() { */ Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Symbol key('x',1); - PriorFactor factor(key, prior, model); + auto model = noiseModel::Isotropic::Sigma(1, 1 * degree); + Symbol key('x', 1); /** * Step 2: Create a graph container and add the factor to it @@ -90,7 +88,7 @@ int main() { * many more factors would be added. */ NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior(key, prior, model); graph.print("full graph"); /** diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 80ac08e030..d1155fe4cf 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -34,7 +34,6 @@ #include #include #include -#include #include #include #include @@ -58,9 +57,8 @@ #include #ifdef GTSAM_USE_TBB -#include -#undef max // TBB seems to include windows.h and we don't want these macros -#undef min +#include // tbb::task_arena +#include // tbb::task_group #endif using namespace std; @@ -206,10 +204,11 @@ int main(int argc, char *argv[]) { } #ifdef GTSAM_USE_TBB - std::unique_ptr init; + tbb::task_arena arena; + tbb::task_group tg; if(nThreads > 0) { cout << "Using " << nThreads << " threads" << endl; - init.reset(new tbb::task_scheduler_init(nThreads)); + arena.initialize(nThreads); } else cout << "Using threads for all processors" << endl; #else @@ -219,6 +218,10 @@ int main(int argc, char *argv[]) { } #endif +#ifdef GTSAM_USE_TBB + arena.execute([&]{ + tg.run_and_wait([&]{ +#endif // Run mode if(incremental) runIncremental(); @@ -230,6 +233,10 @@ int main(int argc, char *argv[]) { runPerturb(); else if(stats) runStats(); +#ifdef GTSAM_USE_TBB + }); + }); +#endif return 0; } @@ -248,7 +255,7 @@ void runIncremental() cout << "Looking for first measurement from step " << firstStep << endl; size_t nextMeasurement = 0; bool havePreviousPose = false; - Key firstPose; + Key firstPose = 0; while(nextMeasurement < datasetMeasurements.size()) { if(BetweenFactor::shared_ptr factor = @@ -281,7 +288,7 @@ void runIncremental() NonlinearFactorGraph newFactors; Values newVariables; - newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(3))); + newFactors.addPrior(firstPose, Pose(), noiseModel::Unit::Create(3)); newVariables.insert(firstPose, Pose()); isam2.update(newFactors, newVariables); @@ -464,7 +471,7 @@ void runBatch() cout << "Creating batch optimizer..." << endl; NonlinearFactorGraph measurements = datasetMeasurements; - measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(3))); + measurements.addPrior(0, Pose(), noiseModel::Unit::Create(3)); gttic_(Create_optimizer); GaussNewtonParams params; diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 65ab5422d7..4e94155471 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SteroVOExample.cpp + * @file StereoVOExample.cpp * @brief A stereo visual odometry example * @date May 25, 2014 * @author Stephen Camp @@ -34,17 +34,17 @@ using namespace std; using namespace gtsam; -int main(int argc, char** argv){ - - //create graph object, add first pose at origin with key '1' +int main(int argc, char** argv) { + // create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; Pose3 first_pose; graph.emplace_shared >(1, Pose3()); - //create factor noise model with 3 sigmas of value 1 - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); - //create stereo camera calibration object with .2m between cameras - const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); + // create factor noise model with 3 sigmas of value 1 + const auto model = noiseModel::Isotropic::Sigma(3, 1); + // create stereo camera calibration object with .2m between cameras + const Cal3_S2Stereo::shared_ptr K( + new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); //create and add stereo factors between first pose (key value 1) and the three landmarks graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); @@ -56,17 +56,18 @@ int main(int argc, char** argv){ graph.emplace_shared >(StereoPoint2(70, 20, 490), model, 2, 4, K); graph.emplace_shared >(StereoPoint2(320, 270, 115), model, 2, 5, K); - //create Values object to contain initial estimates of camera poses and landmark locations + // create Values object to contain initial estimates of camera poses and + // landmark locations Values initial_estimate; - //create and add iniital estimates + // create and add iniital estimates initial_estimate.insert(1, first_pose); initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1))); initial_estimate.insert(3, Point3(1, 1, 5)); initial_estimate.insert(4, Point3(-1, 1, 5)); initial_estimate.insert(5, Point3(0, -0.5, 5)); - //create Levenberg-Marquardt optimizer for resulting factor graph, optimize + // create Levenberg-Marquardt optimizer for resulting factor graph, optimize LevenbergMarquardtOptimizer optimizer(graph, initial_estimate); Values result = optimizer.optimize(); diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 5eeb6ac3cc..af81db7030 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** -* @file SteroVOExample.cpp +* @file StereoVOExample_large.cpp * @brief A stereo visual odometry example * @date May 25, 2014 * @author Stephen Camp @@ -41,31 +41,31 @@ using namespace std; using namespace gtsam; -int main(int argc, char** argv){ - +int main(int argc, char** argv) { Values initial_estimate; NonlinearFactorGraph graph; - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + const auto model = noiseModel::Isotropic::Sigma(3, 1); string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - //read camera calibration info from file + // read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; ifstream calibration_file(calibration_loc.c_str()); cout << "Reading calibration info" << endl; calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; - //create stereo camera calibration object - const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); + // create stereo camera calibration object + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b)); ifstream pose_file(pose_loc.c_str()); cout << "Reading camera poses" << endl; int pose_id; - MatrixRowMajor m(4,4); - //read camera pose parameters and use to make initial estimates of camera poses + MatrixRowMajor m(4, 4); + // read camera pose parameters and use to make initial estimates of camera + // poses while (pose_file >> pose_id) { for (int i = 0; i < 16; i++) { pose_file >> m.data()[i]; @@ -76,33 +76,37 @@ int main(int argc, char** argv){ // camera and landmark keys size_t x, l; - // pixel coordinates uL, uR, v (same for left/right images due to rectification) - // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + // pixel coordinates uL, uR, v (same for left/right images due to + // rectification) landmark coordinates X, Y, Z in camera frame, resulting from + // triangulation double uL, uR, v, X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; - //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation + // read stereo measurement details from file and use to create and add + // GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { - graph.emplace_shared< - GenericStereoFactor >(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K); - //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it + graph.emplace_shared >( + StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); + // if the landmark variable included in this factor has not yet been added + // to the initial variable value estimate, add it if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space + // transformFrom() transforms the input Point3 from the camera pose space, + // camPose, to the global space Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); initial_estimate.insert(Symbol('l', l), worldPoint); } } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); - //constrain the first pose such that it cannot change from its original value during optimization + Pose3 first_pose = initial_estimate.at(Symbol('x', 1)); + // constrain the first pose such that it cannot change from its original value + // during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.emplace_shared >(Symbol('x',1),first_pose); + graph.emplace_shared >(Symbol('x', 1), first_pose); cout << "Optimizing" << endl; - //create Levenberg-Marquardt optimizer to optimize the factor graph + // create Levenberg-Marquardt optimizer to optimize the factor graph LevenbergMarquardtParams params; params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params); diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index 178fa513f6..4bc5144f44 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -28,9 +28,12 @@ using boost::assign::list_of; #ifdef GTSAM_USE_TBB -#include -#undef max // TBB seems to include windows.h and we don't want these macros -#undef min +#include // tbb::blocked_range +#include // tbb::tick_count +#include // tbb::parallel_for +#include // tbb::cache_aligned_allocator +#include // tbb::task_arena +#include // tbb::task_group static const DenseIndex numberOfProblems = 1000000; static const DenseIndex problemSize = 4; @@ -67,10 +70,14 @@ struct WorkerWithoutAllocation }; /* ************************************************************************* */ -map testWithoutMemoryAllocation() +map testWithoutMemoryAllocation(int num_threads) { // A function to do some matrix operations without allocating any memory + // Create task_arena and task_group + tbb::task_arena arena(num_threads); + tbb::task_group tg; + // Now call it vector results(numberOfProblems); @@ -79,7 +86,14 @@ map testWithoutMemoryAllocation() for(size_t grainSize: grainSizes) { tbb::tick_count t0 = tbb::tick_count::now(); - tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithoutAllocation(results)); + + // Run parallel code (as a task group) inside of task arena + arena.execute([&]{ + tg.run_and_wait([&]{ + tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithoutAllocation(results)); + }); + }); + tbb::tick_count t1 = tbb::tick_count::now(); cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; timingResults[(int)grainSize] = (t1 - t0).seconds(); @@ -120,10 +134,14 @@ struct WorkerWithAllocation }; /* ************************************************************************* */ -map testWithMemoryAllocation() +map testWithMemoryAllocation(int num_threads) { // A function to do some matrix operations with allocating memory + // Create task_arena and task_group + tbb::task_arena arena(num_threads); + tbb::task_group tg; + // Now call it vector results(numberOfProblems); @@ -132,7 +150,14 @@ map testWithMemoryAllocation() for(size_t grainSize: grainSizes) { tbb::tick_count t0 = tbb::tick_count::now(); - tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); + + // Run parallel code (as a task group) inside of task arena + arena.execute([&]{ + tg.run_and_wait([&]{ + tbb::parallel_for(tbb::blocked_range(0, numberOfProblems), WorkerWithAllocation(results)); + }); + }); + tbb::tick_count t1 = tbb::tick_count::now(); cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl; timingResults[(int)grainSize] = (t1 - t0).seconds(); @@ -153,9 +178,8 @@ int main(int argc, char* argv[]) for(size_t n: numThreads) { cout << "With " << n << " threads:" << endl; - tbb::task_scheduler_init init((int)n); - results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation(); - results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation(); + results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation((int)n); + results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation((int)n); cout << endl; } diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 4ce4e7af46..3a885a844c 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 @@ -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 f5338bc678..27a6205a39 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/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 35bc9bcf6c..b05fd9e6f4 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -47,7 +47,6 @@ // Adjustment problems. Here we will use Projection factors to model the // camera's landmark observations. Also, we will initialize the robot at some // location using a Prior factor. -#include #include #include @@ -89,7 +88,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) { // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.emplace_shared >( measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); @@ -110,13 +109,11 @@ int main(int argc, char* argv[]) { static auto kPosePrior = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) .finished()); - graph.emplace_shared >(Symbol('x', 0), poses[0], - kPosePrior); + graph.addPrior(Symbol('x', 0), poses[0], kPosePrior); // Add a prior on landmark l0 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], - kPointPrior); + graph.addPrior(Symbol('l', 0), points[0], kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 7a58deeb74..c18781169b 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -38,7 +38,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Projection factors to model the camera's landmark observations. // Also, we will initialize the robot at some location using a Prior factor. -#include #include // We want to use iSAM to solve the structure-from-motion problem incrementally, so @@ -57,13 +56,11 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Define the camera calibration parameters Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr noise = // - noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks vector points = createPoints(); @@ -82,11 +79,10 @@ int main(int argc, char* argv[]) { // Loop over the different poses, adding the observations to iSAM incrementally for (size_t i = 0; i < poses.size(); ++i) { - // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { // Create ground truth measurement - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement graph.emplace_shared >(measurement, noise, @@ -106,14 +102,14 @@ int main(int argc, char* argv[]) { // adding it to iSAM. if (i == 0) { // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + auto poseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); + graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 - noiseModel::Isotropic::shared_ptr pointNoise = + auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); + graph.addPrior(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 49b99a5b6e..7693fa4e4e 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 1dde7efb56..a2ad8cf0b0 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -20,7 +20,7 @@ * @author Stephen Williams */ -#include +#include #include //#include #include diff --git a/gtsam.h b/gtsam.h index 062bcb5314..2cd30be42e 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; @@ -639,6 +640,7 @@ class Rot3 { static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) static gtsam::Rot3 Ypr(double y, double p, double r); static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); @@ -674,6 +676,7 @@ class Rot3 { double roll() const; double pitch() const; double yaw() const; + pair axisAngle() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; @@ -1310,7 +1313,7 @@ class SymbolicBayesTree { // class SymbolicBayesTreeClique { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const std::pair& result) : Base(result) {} +// // BayesTreeClique(const pair& result) : Base(result) {} // // bool equals(const This& other, double tol) const; // void print(string s) const; @@ -1456,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 { @@ -1467,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 { @@ -1478,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 { @@ -1489,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 { @@ -1500,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 { @@ -1511,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 { @@ -1522,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 { @@ -1533,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 { @@ -1544,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 @@ -1561,17 +1564,15 @@ virtual class Robust : gtsam::noiseModel::Base { #include class Sampler { - //Constructors + // Constructors Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(Vector sigmas, int seed); - Sampler(int seed); - //Standard Interface + // Standard Interface size_t dim() const; Vector sigmas() const; gtsam::noiseModel::Diagonal* model() const; Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; #include @@ -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(); @@ -1977,6 +1994,35 @@ size_t symbol(char chr, size_t index); char symbolChr(size_t key); size_t symbolIndex(size_t key); +namespace symbol_shorthand { + size_t A(size_t j); + size_t B(size_t j); + size_t C(size_t j); + size_t D(size_t j); + size_t E(size_t j); + size_t F(size_t j); + size_t G(size_t j); + size_t H(size_t j); + size_t I(size_t j); + size_t J(size_t j); + size_t K(size_t j); + size_t L(size_t j); + size_t M(size_t j); + size_t N(size_t j); + size_t O(size_t j); + size_t P(size_t j); + size_t Q(size_t j); + size_t R(size_t j); + size_t S(size_t j); + size_t T(size_t j); + size_t U(size_t j); + size_t V(size_t j); + size_t W(size_t j); + size_t X(size_t j); + size_t Y(size_t j); + size_t Z(size_t j); +}///\namespace symbol + // Default keyformatter void PrintKeyList (const gtsam::KeyList& keys); void PrintKeyList (const gtsam::KeyList& keys, string s); @@ -2050,6 +2096,9 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; + template + void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + // NonlinearFactorGraph void printErrors(const gtsam::Values& values) const; double error(const gtsam::Values& values) const; @@ -2136,8 +2185,9 @@ class Values { void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::SimpleCamera& simpel_camera); + void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, Vector vector); void insert(size_t j, Matrix matrix); @@ -2155,10 +2205,11 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double @@ -2258,8 +2309,10 @@ virtual class NonlinearOptimizerParams { }; bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); #include virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { @@ -2457,7 +2510,7 @@ class ISAM2 { template + gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -2494,8 +2547,8 @@ class NonlinearISAM { #include #include -#include -template +#include +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2518,7 +2571,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2544,9 +2597,9 @@ typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -2637,7 +2690,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); gtsam::Point2 measured() const; }; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; @@ -2759,8 +2812,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, // std::vector::shared_ptr> class BetweenFactorPose3s { + BetweenFactorPose3s(); size_t size() const; gtsam::BetweenFactorPose3* at(size_t i) const; + void push_back(const gtsam::BetweenFactorPose3* factor); }; #include @@ -2797,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 //************************************************************************* @@ -2910,11 +2993,14 @@ class PreintegratedImuMeasurements { void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + Matrix preintMeasCov() const; double deltaTij() const; 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; @@ -2969,11 +3055,14 @@ class PreintegratedCombinedMeasurements { void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + Matrix preintMeasCov() const; double deltaTij() const; 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; @@ -3151,7 +3240,7 @@ namespace utilities { void perturbPoint2(gtsam::Values& values, double sigma, int seed); void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index ed18b7aad2..c8fecc3397 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -1,6 +1,6 @@ # install CCOLAMD headers -install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD) -install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config) +install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/CCOLAMD) +install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/SuiteSparse_config) if(NOT GTSAM_USE_SYSTEM_EIGEN) # Find plain .h files @@ -12,12 +12,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) get_filename_component(filename ${eigen_dir} NAME) if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) list(APPEND eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/${filename}") - install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen) + install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/Eigen) endif() 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") @@ -26,7 +26,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) get_filename_component(filename ${unsupported_eigen_dir} NAME) if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES "CXX11") OR (${filename} MATCHES ".svn"))) list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") - install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/Eigen) endif() endforeach(unsupported_eigen_dir) endif() @@ -37,12 +37,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) # install Eigen - only the headers in our 3rdparty directory install(DIRECTORY Eigen/Eigen - DESTINATION include/gtsam/3rdparty/Eigen + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") if(GTSAM_WITH_EIGEN_UNSUPPORTED) install(DIRECTORY Eigen/unsupported/Eigen - DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/ FILES_MATCHING PATTERN "*.h") endif() diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt index 9887d58043..e2f1dd6b40 100644 --- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt @@ -39,9 +39,9 @@ endif() add_dependencies(blas eigen_blas eigen_blas_static) install(TARGETS eigen_blas eigen_blas_static - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) if(EIGEN_Fortran_COMPILER_WORKS) diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt index 6df1fa9584..9aa209faaa 100644 --- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt @@ -103,9 +103,9 @@ endif() add_dependencies(lapack eigen_lapack eigen_lapack_static) install(TARGETS eigen_lapack eigen_lapack_static - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/gtsam/3rdparty/metis/GKlib/CMakeLists.txt b/gtsam/3rdparty/metis/GKlib/CMakeLists.txt index 67b600aa6c..ce9fa1124a 100644 --- a/gtsam/3rdparty/metis/GKlib/CMakeLists.txt +++ b/gtsam/3rdparty/metis/GKlib/CMakeLists.txt @@ -16,6 +16,6 @@ include_directories("test") add_subdirectory("test") install(TARGETS GKlib - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib) -install(FILES ${GKlib_includes} DESTINATION include) + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) +install(FILES ${GKlib_includes} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index fdf5e75115..330e989fa9 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -4,23 +4,26 @@ include_directories(.) file(GLOB metis_sources *.c) # Build libmetis. add_definitions(-fPIC) -add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) +add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources}) if(UNIX) - target_link_libraries(metis m) + target_link_libraries(metis-gtsam m) endif() if(WIN32) - set_target_properties(metis PROPERTIES + set_target_properties(metis-gtsam PROPERTIES PREFIX "" RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") endif() if (APPLE) - set_target_properties(metis PROPERTIES + set_target_properties(metis-gtsam PROPERTIES INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib") endif() -install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) -list(APPEND GTSAM_EXPORTED_TARGETS metis) +install(TARGETS metis-gtsam EXPORT GTSAM-exports + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) diff --git a/gtsam/3rdparty/metis/programs/CMakeLists.txt b/gtsam/3rdparty/metis/programs/CMakeLists.txt index 3aaf3577da..67af1d1177 100644 --- a/gtsam/3rdparty/metis/programs/CMakeLists.txt +++ b/gtsam/3rdparty/metis/programs/CMakeLists.txt @@ -15,7 +15,7 @@ endforeach(prog) if(METIS_INSTALL) install(TARGETS gpmetis ndmetis mpmetis m2gmetis graphchk cmpfillin - RUNTIME DESTINATION bin) + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) endif() # Try to find subversion revision. diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1079b845f4..16dca6736f 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -15,7 +15,7 @@ set (gtsam_subdirs sfm slam smart - navigation + navigation ) set(gtsam_srcs) @@ -49,7 +49,7 @@ endif() # Common headers file(GLOB gtsam_core_headers "*.h") -install(FILES ${gtsam_core_headers} DESTINATION include/gtsam) +install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) # assemble core libaries foreach(subdir ${gtsam_subdirs}) @@ -86,10 +86,10 @@ configure_file(config.h.in config.h) set(library_name GTSAM) # For substitution in dllexport.h.in configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h") -install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam) +install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) if(GTSAM_SUPPORT_NESTED_DISSECTION) - list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam) endif() # Versions @@ -172,21 +172,31 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with endif() endif() +if(WIN32) # library to help with demangling variable names on Windows + target_link_libraries(gtsam PRIVATE Dbghelp) +endif() + install( TARGETS gtsam EXPORT GTSAM-exports - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin) + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 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 aee6c0e622..2ac3eb80c1 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -83,7 +84,7 @@ class GenericValue: public Value { /// Virtual print function, uses traits virtual void print(const std::string& str) const { - std::cout << "(" << typeid(T).name() << ") "; + std::cout << "(" << demangle(typeid(T).name()) << ") "; traits::Print(value_, str); } @@ -180,7 +181,7 @@ class GenericValue: public Value { // 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/Lie.h b/gtsam/base/Lie.h index 26c8a80d26..fe730c9340 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -170,7 +170,7 @@ namespace internal { /// Assumes existence of: identity, dimension, localCoordinates, retract, /// and additionally Logmap, Expmap, compose, between, and inverse template -struct LieGroupTraits { +struct LieGroupTraits: GetDimensionImpl { typedef lie_group_tag structure_category; /// @name Group @@ -186,8 +186,6 @@ struct LieGroupTraits { typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - static int GetDimension(const Class&) {return dimension;} - static TangentVector Local(const Class& origin, const Class& other, ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other, Horigin, Hother); diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 21df905130..9feb2b4514 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -72,7 +72,7 @@ struct HasManifoldPrereqs { /// Extra manifold traits for fixed-dimension types template -struct ManifoldImpl { +struct GetDimensionImpl { // Compile-time dimensionality static int GetDimension(const Class&) { return N; @@ -81,7 +81,7 @@ struct ManifoldImpl { /// Extra manifold traits for variable-dimension types template -struct ManifoldImpl { +struct GetDimensionImpl { // Run-time dimensionality static int GetDimension(const Class& m) { return m.dim(); @@ -92,7 +92,7 @@ struct ManifoldImpl { /// To use this for your class type, define: /// template<> struct traits : public internal::ManifoldTraits { }; template -struct ManifoldTraits: ManifoldImpl { +struct ManifoldTraits: GetDimensionImpl { // Check that Class has the necessary machinery BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); @@ -214,7 +214,7 @@ class ProductManifold: public std::pair { 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.cpp b/gtsam/base/Matrix.cpp index f9a2e83a93..e2d8f71d1e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -144,10 +144,10 @@ void print(const Matrix& A, const string &s, ostream& stream) { 0, // flags ", ", // coeffSeparator ";\n", // rowSeparator - " \t", // rowPrefix + "\t", // rowPrefix "", // rowSuffix "[\n", // matPrefix - "\n ]" // matSuffix + "\n]" // matSuffix ); cout << s << A.format(matlab) << endl; } diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 2910ce74c9..1c11384389 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Alex Cunningham * @author Alex Hagiopol + * @author Varun Agrawal */ // \callgraph @@ -88,10 +89,9 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i tol) + if(!fpEqual(A(i,j), B(i,j), tol)) { return false; + } } return true; } @@ -548,17 +548,47 @@ GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); namespace boost { namespace serialization { + /** + * Ref. https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063 + * + * Eigen supports calling resize() on both static and dynamic matrices. + * This allows for a uniform API, with resize having no effect if the static matrix + * is already the correct size. + * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing + * + * We use all the Matrix template parameters to ensure wide compatibility. + * + * eigen_typekit in ROS uses the same code + * http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html + */ + // split version - sends sizes ahead - template - void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) { + template + void save(Archive & ar, + const Eigen::Matrix & m, + const unsigned int /*version*/) { const size_t rows = m.rows(), cols = m.cols(); ar << BOOST_SERIALIZATION_NVP(rows); ar << BOOST_SERIALIZATION_NVP(cols); ar << make_nvp("data", make_array(m.data(), m.size())); } - template - void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) { + template + void load(Archive & ar, + Eigen::Matrix & m, + const unsigned int /*version*/) { size_t rows, cols; ar >> BOOST_SERIALIZATION_NVP(rows); ar >> BOOST_SERIALIZATION_NVP(cols); @@ -566,8 +596,25 @@ namespace boost { ar >> make_nvp("data", make_array(m.data(), m.size())); } - } // namespace serialization -} // namespace boost + // 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); + } -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix); + // 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 diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 82a5ae7f48..602c5915e6 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -55,7 +55,7 @@ class OptionalJacobian { } // Private and very dangerous constructor straight from memory - OptionalJacobian(double* data) : map_(NULL) { + OptionalJacobian(double* data) : map_(nullptr) { if (data) usurp(data); } @@ -66,25 +66,25 @@ class OptionalJacobian { /// Default constructor acts like boost::none OptionalJacobian() : - map_(NULL) { + map_(nullptr) { } /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Jacobian& fixed) : - map_(NULL) { + map_(nullptr) { usurp(fixed.data()); } /// Constructor that will usurp data of a fixed-size matrix, pointer version OptionalJacobian(Jacobian* fixedPtr) : - map_(NULL) { + map_(nullptr) { if (fixedPtr) usurp(fixedPtr->data()); } /// Constructor that will resize a dynamic matrix (unless already correct) OptionalJacobian(Eigen::MatrixXd& dynamic) : - map_(NULL) { + map_(nullptr) { dynamic.resize(Rows, Cols); // no malloc if correct size usurp(dynamic.data()); } @@ -93,12 +93,12 @@ class OptionalJacobian { /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t /*none*/) : - map_(NULL) { + map_(nullptr) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - map_(NULL) { + map_(nullptr) { if (optional) { optional->resize(Rows, Cols); usurp(optional->data()); @@ -110,11 +110,11 @@ class OptionalJacobian { /// Constructor that will usurp data of a block expression /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible // template - // OptionalJacobian(Eigen::Block block) : map_(NULL) { ?? } + // OptionalJacobian(Eigen::Block block) : map_(nullptr) { ?? } /// Return true is allocated, false if default constructor was used operator bool() const { - return map_.data() != NULL; + return map_.data() != nullptr; } /// De-reference, like boost optional @@ -173,7 +173,7 @@ class OptionalJacobian { /// Default constructor acts like boost::none OptionalJacobian() : - pointer_(NULL) { + pointer_(nullptr) { } /// Construct from pointer to dynamic matrix @@ -186,12 +186,12 @@ class OptionalJacobian { /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t /*none*/) : - pointer_(NULL) { + pointer_(nullptr) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - pointer_(NULL) { + pointer_(nullptr) { if (optional) pointer_ = &(*optional); } @@ -199,7 +199,7 @@ class OptionalJacobian { /// Return true is allocated, false if default constructor was used operator bool() const { - return pointer_!=NULL; + return pointer_!=nullptr; } /// De-reference, like boost optional diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 53a8912f67..4e030606da 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -416,4 +416,3 @@ namespace gtsam { class CholeskyFailed; } - diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index 15a76fbf1e..0f7c6131d0 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -11,7 +11,7 @@ /** * @file ThreadSafeException.h - * @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB + * @brief Base exception type that uses tbb_allocator if GTSAM is compiled with TBB * @author Richard Roberts * @date Aug 21, 2010 * @addtogroup base @@ -25,34 +25,28 @@ #include #include #include +#include #ifdef GTSAM_USE_TBB #include -#include #include #include #endif namespace gtsam { -/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. +/// Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. template class ThreadsafeException: -#ifdef GTSAM_USE_TBB - public tbb::tbb_exception -#else public std::exception -#endif { -#ifdef GTSAM_USE_TBB private: - typedef tbb::tbb_exception Base; + typedef std::exception Base; +#ifdef GTSAM_USE_TBB protected: typedef std::basic_string, tbb::tbb_allocator > String; #else -private: - typedef std::exception Base; protected: typedef std::string String; #endif @@ -82,36 +76,6 @@ public std::exception } public: - // Implement functions for tbb_exception -#ifdef GTSAM_USE_TBB - virtual tbb::tbb_exception* move() throw () { - void* cloneMemory = scalable_malloc(sizeof(DERIVED)); - if (!cloneMemory) { - std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; - exit(-1); - } - DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); - clone->dynamic_ = true; - return clone; - } - - virtual void destroy() throw () { - if (dynamic_) { - DERIVED* derivedPtr = static_cast(this); - derivedPtr->~DERIVED(); - scalable_free(derivedPtr); - } - } - - virtual void throw_self() { - throw *static_cast(this); - } - - virtual const char* name() const throw () { - return typeid(DERIVED).name(); - } -#endif - virtual const char* what() const throw () { return description_ ? description_->c_str() : ""; } diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ac03c0f53f..beec030baf 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -14,6 +14,7 @@ * @brief typedef and functions to augment Eigen's Vectors * @author Kai Ni * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -33,6 +34,45 @@ using namespace std; namespace gtsam { +/* ************************************************************************* + * References: + * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * 2. https://floating-point-gui.de/errors/comparison/ + * ************************************************************************* */ +bool fpEqual(double a, double b, double tol) { + using std::abs; + using std::isnan; + using std::isinf; + + double DOUBLE_MIN_NORMAL = numeric_limits::min() + 1.0; + double larger = (abs(b) > abs(a)) ? abs(b) : abs(a); + + // handle NaNs + if(std::isnan(a) || isnan(b)) { + return isnan(a) && isnan(b); + } + // handle inf + else if(isinf(a) || isinf(b)) { + return isinf(a) && isinf(b); + } + // If the two values are zero or both are extremely close to it + // relative error is less meaningful here + else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) { + return abs(a-b) <= tol * DOUBLE_MIN_NORMAL; + } + // Check if the numbers are really close + // Needed when comparing numbers near zero or tol is in vicinity + else if(abs(a-b) <= tol) { + return true; + } + // Use relative error + else if(abs(a-b) <= tol * min(larger, std::numeric_limits::max())) { + return true; + } + + return false; +} + /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { @@ -82,9 +122,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i tol) + if (!fpEqual(vec1[i], vec2[i], tol)) return false; } return true; @@ -95,9 +133,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i tol) + if (!fpEqual(vec1[i], vec2[i], tol)) return false; } return true; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 99c5a4d425..81be36c0ac 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -15,6 +15,7 @@ * @author Kai Ni * @author Frank Dellaert * @author Alex Hagiopol + * @author Varun Agrawal */ // \callgraph @@ -75,6 +76,19 @@ static_assert( "Error: GTSAM was built against a different version of Eigen"); #endif +/** + * Numerically stable function for comparing if floating point values are equal + * within epsilon tolerance. + * Used for vector and matrix comparison with C++11 compatible functions. + * + * If either value is NaN or Inf, we check for both values to be NaN or Inf + * respectively for the comparison to be true. + * If one is NaN/Inf and the other is not, returns false. + * + * Return true if two numbers are close wrt tol. + */ +GTSAM_EXPORT bool fpEqual(double a, double b, double tol); + /** * print without optional string, must specify cout yourself */ diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index d6d2a4fe02..592b42d151 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -20,7 +20,7 @@ #include // for GTSAM_USE_TBB #ifdef GTSAM_USE_TBB -#include +#include // std::mutex, std::unique_lock #endif namespace gtsam { @@ -28,13 +28,13 @@ namespace gtsam { GTSAM_EXPORT FastMap > debugFlags; #ifdef GTSAM_USE_TBB -tbb::mutex debugFlagsMutex; +std::mutex debugFlagsMutex; #endif /* ************************************************************************* */ bool guardedIsDebug(const std::string& s) { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(debugFlagsMutex); + std::unique_lock lock(debugFlagsMutex); #endif return gtsam::debugFlags[s]; } @@ -42,7 +42,7 @@ bool guardedIsDebug(const std::string& s) { /* ************************************************************************* */ void guardedSetDebug(const std::string& s, const bool v) { #ifdef GTSAM_USE_TBB - tbb::mutex::scoped_lock lock(debugFlagsMutex); + std::unique_lock lock(debugFlagsMutex); #endif gtsam::debugFlags[s] = v; } diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h new file mode 100644 index 0000000000..5281eec6de --- /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 e475465deb..f589ecc5ec 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") { +/// serializes to a string +template +std::string serialize(const T& input) { + return serializeToString(input); +} + +/// deserializes from a string +template +void deserialize(const std::string& serialized, T& output) { + deserializeFromString(serialized, output); +} +///@} + +/** @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); +} + +/// 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); +} + +/// serializes to a string in XML +template +std::string serializeToXMLString(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); - } + serializeToXMLStream(input, out_archive_stream, name); return out_archive_stream.str(); } -template -void deserializeXML(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::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + deserializeFromXMLStream(in_archive_stream, output, name); } -template -bool serializeToXMLFile(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::xml_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 deserializeFromXMLFile(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::xml_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; } -// Serialization to binary format with named structures -template -std::string serializeBinary(const T& input, const std::string& name="data") { - std::ostringstream out_archive_stream; +/// 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); - return out_archive_stream.str(); } -template -void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { - std::istringstream in_archive_stream(serialized); +/// 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); } -template -bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { +/// 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; - 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; + serializeToBinaryStream(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 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; - 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; + deserializeFromBinaryStream(in_archive_stream, output, name); in_archive_stream.close(); return true; } -} // \namespace gtsam +/// 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 06b3462e9b..5994a5e51b 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/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 9b2dae3d0f..b52d44e2ab 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,11 +22,8 @@ #include #ifdef GTSAM_USE_TBB -# include -# include -# undef max // TBB seems to include windows.h and we don't want these macros -# undef min -# undef ERROR +#include // tbb::task, tbb::task_list +#include // tbb::scalable_allocator namespace gtsam { @@ -35,28 +32,6 @@ namespace gtsam { namespace internal { - /* ************************************************************************* */ - template - class PostOrderTask : public tbb::task - { - public: - const boost::shared_ptr& treeNode; - boost::shared_ptr myData; - VISITOR_POST& visitorPost; - - PostOrderTask(const boost::shared_ptr& treeNode, - const boost::shared_ptr& myData, VISITOR_POST& visitorPost) - : treeNode(treeNode), myData(myData), visitorPost(visitorPost) {} - - tbb::task* execute() - { - // Run the post-order visitor - (void) visitorPost(treeNode, *myData); - - return NULL; - } - }; - /* ************************************************************************* */ template class PreOrderTask : public tbb::task @@ -88,7 +63,7 @@ namespace gtsam { { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } else { @@ -129,14 +104,14 @@ namespace gtsam { { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return NULL; + return nullptr; } } } @@ -184,8 +159,8 @@ namespace gtsam { set_ref_count(1 + (int) roots.size()); // Spawn tasks spawn_and_wait_for_all(tasks); - // Return NULL - return NULL; + // Return nullptr + return nullptr; } }; diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp new file mode 100644 index 0000000000..edc449b12a --- /dev/null +++ b/gtsam/base/types.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 types.cpp + * @brief Functions for handling type information + * @author Varun Agrawal + * @date May 18, 2020 + * @addtogroup base + */ + +#include + +#ifdef _WIN32 +#define WIN32_LEAN_AND_MEAN +#include +#include +#endif + +#ifdef __GNUG__ +#include +#include +#include +#endif + +namespace gtsam { + +/// Pretty print Value type name +std::string demangle(const char* name) { + // by default set to the original mangled name + std::string demangled_name = std::string(name); + +#ifdef __GNUG__ + // g++ version of demangle + char* demangled = nullptr; + int status = -1; // some arbitrary value to eliminate the compiler warning + demangled = abi::__cxa_demangle(name, nullptr, nullptr, &status), + + demangled_name = (status == 0) ? std::string(demangled) : std::string(name); + + std::free(demangled); + +#elif _WIN32 + char undecorated_name[1024]; + + if (UnDecorateSymbolName( + name, undecorated_name, sizeof(undecorated_name), + UNDNAME_COMPLETE)) + { + // successful conversion, take value from: undecorated_name + demangled_name = std::string(undecorated_name); + } + // else keep using mangled name +#endif + + return demangled_name; +} + +} /* namespace gtsam */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 3b6c9f337c..aaada3ceec 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -27,9 +27,10 @@ #include #include +#include +#include + #ifdef GTSAM_USE_TBB -#include -#include #include #endif @@ -53,6 +54,9 @@ namespace gtsam { + /// Function to demangle type name of variable, e.g. demangle(typeid(x).name()) + std::string GTSAM_EXPORT demangle(const char* name); + /// Integer nonlinear key type typedef std::uint64_t Key; @@ -227,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/config.h.in b/gtsam/config.h.in index 4b8bd180d3..b480996ecc 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -42,6 +42,9 @@ // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) #cmakedefine GTSAM_USE_TBB +// Whether we are using a TBB version higher than 2020 +#cmakedefine TBB_GREATER_EQUAL_2020 + // Whether we are using system-Eigen or our own patched version #cmakedefine GTSAM_USE_SYSTEM_EIGEN diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 041d4c206e..9cc55ed6aa 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -101,7 +101,7 @@ namespace gtsam { /** Create a new function splitting on a variable */ template AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : - Super(NULL) { + Super(nullptr) { this->root_ = compose(begin, end, label); } diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index dcc336f893..237caf7451 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 bed50a4704..990d10dbe6 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 0df6ab4765..3d6e016fdb 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; - - /* ************************************************************************* */ - /** 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; - - 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; - }; - -} +// 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) {} + + /// print index signature only + void printSignature( + const std::string& s = "Clique: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const { + conditional_->printSignature(s, formatter); + } + + //** evaluate conditional probability of subtree for given Values */ + double evaluate(const DiscreteConditional::Values& values) const; +}; + +/* ************************************************************************* */ +/** A Bayes tree representing a Discrete density */ +class GTSAM_EXPORT DiscreteBayesTree + : public BayesTree { + private: + typedef BayesTree Base; + + 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 2ab3054a8e..ac7c584058 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 3da8d0a82f..225e6e1d3d 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -24,6 +24,8 @@ #include #include +#include + namespace gtsam { /** @@ -92,6 +94,13 @@ class GTSAM_EXPORT DiscreteConditional: public DecisionTreeFactor, /// @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/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index af11d4b140..e41968d6b4 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -71,7 +71,7 @@ namespace gtsam { for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter); } } diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index c4cdbe0ef1..fe99ea9752 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); - } - - /* ******************************************************************************** */ - Potentials::Potentials() : - ADT(1.0) { - } - - /* ******************************************************************************** */ - Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) : - ADT(decisionTree), cardinalities_(keys.cardinalities()) { - } +/* ************************************************************************* */ +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); +} - /* ************************************************************************* */ - bool Potentials::equals(const Potentials& other, double tol) const { - return ADT::equals(other, tol); - } +/* ******************************************************************************** + */ +Potentials::Potentials() : ADT(1.0) {} - /* ************************************************************************* */ - 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(" "); - } +/* ******************************************************************************** + */ +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); +} + +/* ************************************************************************* */ +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 89e7637030..94b160a291 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 587cd6b30d..6c59b5bffa 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 9c3f4bd631..0261ef833f 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -24,7 +24,6 @@ #include // for convert only #define DISABLE_TIMING -#include #include #include #include @@ -32,6 +31,7 @@ using namespace boost::assign; #include #include +#include using namespace std; using namespace gtsam; @@ -75,16 +75,15 @@ void dot(const T&f, const string& filename) { // instrumented operators /* ******************************************************************************** */ size_t muls = 0, adds = 0; -boost::timer timer; +double elapsed; void resetCounts() { muls = 0; adds = 0; - timer.restart(); } void printCounts(const string& s) { #ifndef DISABLE_TIMING cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds - % (1000 * timer.elapsed()) << endl; + % (1000 * elapsed) << endl; #endif resetCounts(); } @@ -133,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(); @@ -148,6 +147,7 @@ TEST(ADT, joint) DiscreteKey A(0, 2), S(1, 2), T(2, 2), L(3, 2), B(4, 2), E(5, 2), X(6, 2), D(7, 2); resetCounts(); + gttic_(asiaCPTs); ADT pA = create(A % "99/1"); ADT pS = create(S % "50/50"); ADT pT = create(T | A = "99/1 95/5"); @@ -156,10 +156,15 @@ TEST(ADT, joint) ADT pE = create((E | T, L) = "F T T T"); ADT pX = create(X | E = "95/5 2/98"); ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); + gttoc_(asiaCPTs); + tictoc_getNode(asiaCPTsNode, asiaCPTs); + elapsed = asiaCPTsNode->secs() + asiaCPTsNode->wall(); + tictoc_reset_(); printCounts("Asia CPTs"); // Create joint resetCounts(); + gttic_(asiaJoint); ADT joint = pA; dot(joint, "Asia-A"); joint = apply(joint, pS, &mul); @@ -176,15 +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_); @@ -199,6 +209,7 @@ TEST(ADT, inference) B(2,2), L(3,2), E(4,2), S(5,2), T(6,2), X(7,2); resetCounts(); + gttic_(infCPTs); ADT pA = create(A % "99/1"); ADT pS = create(S % "50/50"); ADT pT = create(T | A = "99/1 95/5"); @@ -207,10 +218,15 @@ TEST(ADT, inference) ADT pE = create((E | T, L) = "F T T T"); ADT pX = create(X | E = "95/5 2/98"); ADT pD = create((D | E, B) = "9/1 2/8 3/7 1/9"); + gttoc_(infCPTs); + tictoc_getNode(infCPTsNode, infCPTs); + elapsed = infCPTsNode->secs() + infCPTsNode->wall(); + tictoc_reset_(); // printCounts("Inference CPTs"); // Create joint resetCounts(); + gttic_(asiaProd); ADT joint = pA; dot(joint, "Joint-Product-A"); joint = apply(joint, pS, &mul); @@ -228,8 +244,14 @@ TEST(ADT, inference) joint = apply(joint, pD, &mul); dot(joint, "Joint-Product-ASTLBEXD"); EXPECT_LONGS_EQUAL(370, (long)muls); // different ordering + gttoc_(asiaProd); + tictoc_getNode(asiaProdNode, asiaProd); + elapsed = asiaProdNode->secs() + asiaProdNode->wall(); + tictoc_reset_(); printCounts("Asia product"); + resetCounts(); + gttic_(asiaSum); ADT marginal = joint; marginal = marginal.combine(X, &add_); dot(marginal, "Joint-Sum-ADBLEST"); @@ -240,6 +262,10 @@ TEST(ADT, inference) marginal = marginal.combine(E, &add_); dot(marginal, "Joint-Sum-ADBL"); EXPECT_LONGS_EQUAL(161, (long)adds); + gttoc_(asiaSum); + tictoc_getNode(asiaSumNode, asiaSum); + elapsed = asiaSumNode->secs() + asiaSumNode->wall(); + tictoc_reset_(); printCounts("Asia sum"); } @@ -249,6 +275,7 @@ TEST(ADT, factor_graph) DiscreteKey B(0,2), L(1,2), E(2,2), S(3,2), T(4,2), X(5,2); resetCounts(); + gttic_(createCPTs); ADT pS = create(S % "50/50"); ADT pT = create(T % "95/5"); ADT pL = create(L | S = "99/1 90/10"); @@ -256,10 +283,15 @@ TEST(ADT, factor_graph) ADT pX = create(X | E = "95/5 2/98"); ADT pD = create(B | E = "1/8 7/9"); ADT pB = create(B | S = "70/30 40/60"); + gttoc_(createCPTs); + tictoc_getNode(createCPTsNode, createCPTs); + elapsed = createCPTsNode->secs() + createCPTsNode->wall(); + tictoc_reset_(); // printCounts("Create CPTs"); // Create joint resetCounts(); + gttic_(asiaFG); ADT fg = pS; fg = apply(fg, pT, &mul); fg = apply(fg, pL, &mul); @@ -269,8 +301,14 @@ TEST(ADT, factor_graph) fg = apply(fg, pD, &mul); dot(fg, "FactorGraph"); EXPECT_LONGS_EQUAL(158, (long)muls); + gttoc_(asiaFG); + tictoc_getNode(asiaFGNode, asiaFG); + elapsed = asiaFGNode->secs() + asiaFGNode->wall(); + tictoc_reset_(); printCounts("Asia FG"); + resetCounts(); + gttic_(marg); fg = fg.combine(X, &add_); dot(fg, "Marginalized-6X"); fg = fg.combine(T, &add_); @@ -282,49 +320,83 @@ TEST(ADT, factor_graph) fg = fg.combine(L, &add_); dot(fg, "Marginalized-2L"); EXPECT(adds = 54); + gttoc_(marg); + tictoc_getNode(margNode, marg); + elapsed = margNode->secs() + margNode->wall(); + tictoc_reset_(); printCounts("marginalize"); // BLESTX // Eliminate X + resetCounts(); + gttic_(elimX); ADT fE = pX; dot(fE, "Eliminate-01-fEX"); fE = fE.combine(X, &add_); dot(fE, "Eliminate-02-fE"); + gttoc_(elimX); + tictoc_getNode(elimXNode, elimX); + elapsed = elimXNode->secs() + elimXNode->wall(); + tictoc_reset_(); printCounts("Eliminate X"); // Eliminate T + resetCounts(); + gttic_(elimT); ADT fLE = pT; fLE = apply(fLE, pE, &mul); dot(fLE, "Eliminate-03-fLET"); fLE = fLE.combine(T, &add_); dot(fLE, "Eliminate-04-fLE"); + gttoc_(elimT); + tictoc_getNode(elimTNode, elimT); + elapsed = elimTNode->secs() + elimTNode->wall(); + tictoc_reset_(); printCounts("Eliminate T"); // Eliminate S + resetCounts(); + gttic_(elimS); ADT fBL = pS; fBL = apply(fBL, pL, &mul); fBL = apply(fBL, pB, &mul); dot(fBL, "Eliminate-05-fBLS"); fBL = fBL.combine(S, &add_); dot(fBL, "Eliminate-06-fBL"); + gttoc_(elimS); + tictoc_getNode(elimSNode, elimS); + elapsed = elimSNode->secs() + elimSNode->wall(); + tictoc_reset_(); printCounts("Eliminate S"); // Eliminate E + resetCounts(); + gttic_(elimE); ADT fBL2 = fE; fBL2 = apply(fBL2, fLE, &mul); fBL2 = apply(fBL2, pD, &mul); dot(fBL2, "Eliminate-07-fBLE"); fBL2 = fBL2.combine(E, &add_); dot(fBL2, "Eliminate-08-fBL2"); + gttoc_(elimE); + tictoc_getNode(elimENode, elimE); + elapsed = elimENode->secs() + elimENode->wall(); + tictoc_reset_(); printCounts("Eliminate E"); // Eliminate L + resetCounts(); + gttic_(elimL); ADT fB = fBL; fB = apply(fB, fBL2, &mul); dot(fB, "Eliminate-09-fBL"); fB = fB.combine(L, &add_); dot(fB, "Eliminate-10-fB"); + gttoc_(elimL); + tictoc_getNode(elimLNode, elimL); + elapsed = elimLNode->secs() + elimLNode->wall(); + tictoc_reset_(); printCounts("Eliminate L"); } diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 05223c67a7..71cb4abe39 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -17,7 +17,6 @@ * @date Jan 30, 2012 */ -#include #include using namespace boost::assign; diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 5ed6623328..2b440e5a0c 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 93126f6420..ecf485036a 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 0000000000..e8167d4554 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 888bf76df7..3ac3ffc9eb 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 0fbf44097d..1defd5acff 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 4e9f956b6e..e1eb92af3a 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 de47a00f36..049c455f72 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 8214b07273..8db7abffe7 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -162,7 +162,7 @@ struct BearingRange { 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 @@ -187,8 +187,8 @@ struct HasBearing { }; // Similar helper class for to implement Range traits for classes with a range method -// For classes with overloaded range methods, such as SimpleCamera, this can even be templated: -// template struct Range : HasRange {}; +// For classes with overloaded range methods, such as PinholeCamera, this can even be templated: +// template struct Range : HasRange {}; template struct HasRange { typedef RT result_type; diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 4009a19219..3e62f758d4 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -76,10 +76,10 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { Vector localCoordinates(const Cal3DS2& T2) const ; /// Return dimensions of calibration manifold object - virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return dimension ; } /// Return dimensions of calibration manifold object - static size_t Dim() { return 9; } //TODO: make a final dimension variable + static size_t Dim() { return dimension; } /// @} /// @name Clone diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp new file mode 100644 index 0000000000..f7794fafb3 --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye.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 + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye.cpp + * @date Apr 8, 2020 + * @author ghaggin + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Fisheye::Cal3Fisheye(const Vector9& v) + : fx_(v[0]), + fy_(v[1]), + s_(v[2]), + u0_(v[3]), + v0_(v[4]), + k1_(v[5]), + k2_(v[6]), + k3_(v[7]), + k4_(v[8]) {} + +/* ************************************************************************* */ +Vector9 Cal3Fisheye::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_; + return v; +} + +/* ************************************************************************* */ +Matrix3 Cal3Fisheye::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; +} + +/* ************************************************************************* */ +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 r2 = xi * xi + yi * yi, r = sqrt(r2); + const double t = atan(r); + 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) { + 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) { + 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; +} + +/* ************************************************************************* */ +Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { + // initial gues just inverts the pinhole model + const double u = uv.x(), v = uv.y(); + const double yd = (v - v0_) / fy_; + const double xd = (u - s_ * yd - u0_) / fx_; + Point2 pi(xd, yd); + + // Perform newtons method, break when solution converges past tol, + // throw exception if max iterations are reached + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + Matrix2 jac; + + // Calculate the current estimate (uv_hat) and the jacobian + const Point2 uv_hat = uncalibrate(pi, boost::none, jac); + + // Test convergence + if ((uv_hat - uv).norm() < tol) break; + + // Newton's method update step + pi = pi - jac.inverse() * (uv_hat - uv); + } + + if (iteration >= maxIterations) + throw std::runtime_error( + "Cal3Fisheye::calibrate fails to converge. need a better " + "initialization"); + + return pi; +} + +/* ************************************************************************* */ +void Cal3Fisheye::print(const std::string& s_) const { + gtsam::print((Matrix)K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); +} + +/* ************************************************************************* */ +bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const { + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || + std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol || + std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || + std::abs(k4_ - K.k4_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const { + return Cal3Fisheye(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const { + return T2.vector() - vector(); +} + +} // namespace gtsam +/* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h new file mode 100644 index 0000000000..e24fe074f8 --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye.h @@ -0,0 +1,209 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Fisheye.h + * @brief Calibration of a fisheye camera + * @date Apr 8, 2020 + * @author ghaggin + */ + +#pragma once + +#include + +#include + +namespace gtsam { + +/** + * @brief Calibration of a fisheye camera + * @addtogroup geometry + * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html + * 3D point in camera frame + * p = (x, y, z) + * Intrinsic coordinates: + * [x_i;y_i] = [x/z; y/z] + * Distorted coordinates: + * r^2 = (x_i)^2 + (y_i)^2 + * th = atan(r) + * th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8) + * [x_d; y_d] = (th_d / r)*[x_i; y_i] + * Pixel coordinates: + * K = [fx s u0; 0 fy v0 ;0 0 1] + * [u; v; 1] = K*[x_d; y_d; 1] + */ +class GTSAM_EXPORT Cal3Fisheye { + private: + double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point + double k1_, k2_, k3_, k4_; // fisheye distortion coefficients + + public: + enum { dimension = 9 }; + typedef boost::shared_ptr + shared_ptr; ///< shared pointer to fisheye calibration object + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Fisheye() + : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + + Cal3Fisheye(const double fx, const double fy, const double s, const double u0, + const double v0, const double k1, const double k2, + const double k3, const double k4) + : fx_(fx), + fy_(fy), + s_(s), + u0_(u0), + v0_(v0), + k1_(k1), + k2_(k2), + k3_(k3), + k4_(k4) {} + + virtual ~Cal3Fisheye() {} + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit Cal3Fisheye(const Vector9& v); + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_; } + + /// focal length x + inline double fy() const { return fy_; } + + /// skew + inline double skew() const { return s_; } + + /// image center in x + inline double px() const { return u0_; } + + /// image center in y + inline double py() const { return v0_; } + + /// First distortion coefficient + inline double k1() const { return k1_; } + + /// Second distortion coefficient + inline double k2() const { return k2_; } + + /// First tangential distortion coefficient + inline double k3() const { return k3_; } + + /// Second tangential distortion coefficient + inline double k4() const { return k4_; } + + /// return calibration matrix + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); } + + /// 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] + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ..., + * k4) + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) + * @return point in (distorted) image coordinates + */ + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; + + /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, + /// y_i] + Point2 calibrate(const Point2& p, const double tol = 1e-5) const; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + virtual void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const Cal3Fisheye& K, double tol = 10e-9) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Fisheye retract(const Vector& d) const; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Fisheye& T2) const; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 9; } + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 9; } + + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3Fisheye(*this)); + } + + /// @} + + private: + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(fx_); + ar& BOOST_SERIALIZATION_NVP(fy_); + ar& BOOST_SERIALIZATION_NVP(s_); + ar& BOOST_SERIALIZATION_NVP(u0_); + ar& BOOST_SERIALIZATION_NVP(v0_); + ar& BOOST_SERIALIZATION_NVP(k1_); + ar& BOOST_SERIALIZATION_NVP(k2_); + ar& BOOST_SERIALIZATION_NVP(k3_); + ar& BOOST_SERIALIZATION_NVP(k4_); + } + + /// @} +}; + +template <> +struct traits : public internal::Manifold {}; + +template <> +struct traits : public internal::Manifold {}; + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 9982cec478..ae75c3916e 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -118,10 +118,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { Vector10 localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object - virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return dimension ; } /// Return dimensions of calibration manifold object - static size_t Dim() { return 10; } //TODO: make a final dimension variable + static size_t Dim() { return dimension; } /// Return all parameters as a vector Vector10 vector() const ; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 747f53fe17..f2848d0a3f 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -193,14 +193,10 @@ class GTSAM_EXPORT Cal3_S2 { /// @{ /// return DOF, dimensionality of tangent space - inline size_t dim() const { - return 5; - } + inline size_t dim() const { return dimension; } /// return DOF, dimensionality of tangent space - static size_t Dim() { - return 5; - } + static size_t Dim() { return dimension; } /// Given 5-dim tangent vector, create new calibration inline Cal3_S2 retract(const Vector& d) const { diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 51692dc823..a6eb41b60d 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -111,14 +111,10 @@ namespace gtsam { /// @{ /// return DOF, dimensionality of tangent space - inline size_t dim() const { - return 6; - } + inline size_t dim() const { return dimension; } /// return DOF, dimensionality of tangent space - static size_t Dim() { - return 6; - } + static size_t Dim() { return dimension; } /// Given 6-dim tangent vector, create new calibration inline Cal3_S2Stereo retract(const Vector& d) const { diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9d9b37d7a9..eff747eb50 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -326,12 +326,12 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase { /// @deprecated inline size_t dim() const { - return 6; + return dimension; } /// @deprecated inline static size_t Dim() { - return 6; + return dimension; } /// @} diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index ecf9a572db..feab8e0fa0 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -319,7 +319,7 @@ class CameraSet : public std::vector > } 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 3235fdedd1..ca719eb371 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/Line3.cpp b/gtsam/geometry/Line3.cpp new file mode 100644 index 0000000000..e3b4841e00 --- /dev/null +++ b/gtsam/geometry/Line3.cpp @@ -0,0 +1,120 @@ +#include + +namespace gtsam { + +Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> Dp, OptionalJacobian<4, 4> Dv) const { + Vector3 w; + w << v[0], v[1], 0; + Rot3 incR; + + if (Dp) { + Dp->setIdentity(); + incR = Rot3::Expmap(w); + Dp->block<2, 2>(0, 0) = ((incR.matrix()).transpose()).block<2, 2>(0, 0); + } + if (Dv) { + Matrix3 Dw; + incR = Rot3::Expmap(w, Dw); + Dv->setIdentity(); + Dv->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0); + } else { + incR = Rot3::Expmap(w); + } + Rot3 Rt = R_ * incR; + return Line3(Rt, a_ + v[2], b_ + v[3]); +} + +Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> Dp, + OptionalJacobian<4, 4> Dq) const { + Vector3 omega; + Matrix3 D_log; + omega = Rot3::Logmap(R_.inverse() * q.R_, D_log); + if (Dp) { + Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix(); + Matrix3 Dwp = D_log * D_log_wp; + Dp->setIdentity(); + Dp->block<2, 2>(0, 0) = Dwp.block<2, 2>(0, 0); + (*Dp)(2, 2) = -1; + (*Dp)(3, 3) = -1; + } + if (Dq) { + Dq->setIdentity(); + Dq->block<2, 2>(0, 0) = D_log.block<2, 2>(0, 0); + } + Vector4 local; + local << omega[0], omega[1], q.a_ - a_, q.b_ - b_; + return local; +} + +void Line3::print(const std::string &s) const { + std::cout << s << std::endl; + R_.print("R:\n"); + std::cout << "a: " << a_ << ", b: " << b_ << std::endl; +} + +bool Line3::equals(const Line3 &l2, double tol) const { + Vector4 diff = localCoordinates(l2); + return fabs(diff[0]) < tol && fabs(diff[1]) < tol + && fabs(diff[2]) < tol && fabs(diff[3]) < tol; +} + +Unit3 Line3::project(OptionalJacobian<2, 4> Dline) const { + Vector3 V_0; + V_0 << -b_, a_, 0.0; + + Unit3 l; + if (Dline) { + // Jacobian of the normalized Unit3 projected line with respect to + // un-normalized Vector3 projected line in homogeneous coordinates. + Matrix23 D_unit_line; + l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line); + // Jacobian of the un-normalized Vector3 line with respect to + // input 3D line + Matrix34 D_vec_line = Matrix34::Zero(); + D_vec_line.col(0) = a_ * R_.r3(); + D_vec_line.col(1) = b_ * R_.r3(); + D_vec_line.col(2) = R_.r2(); + D_vec_line.col(3) = -R_.r1(); + // Jacobian of output wrt input is the product of the two. + *Dline = D_unit_line * D_vec_line; + } else { + l = Unit3::FromPoint3(Point3(R_ * V_0)); + } + return l; +} + +Point3 Line3::point(double distance) const { + // defining "center" of the line to be the point where it + // intersects rotated XY axis + Point3 center(a_, b_, 0); + Point3 rotated_center = R_ * center; + return rotated_center + distance * R_.r3(); +} + +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) { + Rot3 wRc = wTc.rotation(); + Rot3 cRw = wRc.inverse(); + Rot3 cRl = cRw * wL.R_; + + Vector2 w_ab; + Vector3 t = ((wL.R_).transpose() * wTc.translation()); + Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]); + + if (Dpose) { + Matrix3 lRc = (cRl.matrix()).transpose(); + Dpose->setZero(); + // rotation + Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0); + // translation + Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0); + } + if (Dline) { + Dline->setIdentity(); + (*Dline)(0, 3) = -t[2]; + (*Dline)(1, 2) = t[2]; + } + return Line3(cRl, c_ab[0], c_ab[1]); +} + +} \ No newline at end of file diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h new file mode 100644 index 0000000000..1c7ed2f4c8 --- /dev/null +++ b/gtsam/geometry/Line3.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + * 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 Line3.h + * @brief 4 dimensional manifold of 3D lines + * @author Akshay Krishnan + * @author Frank Dellaert + */ +// \callgraph + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * A 3D line (R,a,b) : (Rot3,Scalar,Scalar) + * @addtogroup geometry + * \nosubgrouping + */ +class Line3 { + private: + Rot3 R_; // Rotation of line about x and y in world frame + double a_, b_; // Intersection of line with the world x-y plane rotated by R_ + // Also the closest point on line to origin + public: + enum { dimension = 4 }; + + /** Default constructor is the Z axis **/ + Line3() : + a_(0), b_(0) {} + + /** Constructor for general line from (R, a, b) **/ + Line3(const Rot3 &R, const double a, const double b) : + R_(R), a_(a), b_(b) {} + + /** + * The retract method maps from the tangent space back to the manifold. + * The method q = p + v, where p is this line, v is an increment along + * the tangent space and q is the resulting line. + * The tangent space for the rotation of a line is only two dimensional - + * rotation about x and y + * @param v: increment in tangent space + * @param Dp: increment of retraction with respect to this line + * @param Dv: Jacobian of retraction with respect to the increment + * @return q: resulting line after adding the increment and mapping to the manifold + */ + Line3 retract(const Vector4 &v, + OptionalJacobian<4, 4> Dp = boost::none, + OptionalJacobian<4, 4> Dv = boost::none) const; + + /** + * The localCoordinates method is the inverse of retract and finds the difference + * between two lines in the tangent space. It computes v = q - p where q is an input + * line, p is this line and v is their difference in the tangent space. + * @param q: Line3 on manifold + * @param Dp: OptionalJacobian of localCoordinates with respect to this line + * @param Dq: OptionalJacobian of localCoordinates with respect to this line + * @return v: difference in the tangent space + */ + Vector4 localCoordinates(const Line3 &q, + OptionalJacobian<4, 4> Dp = boost::none, + OptionalJacobian<4, 4> Dq = boost::none) const; + + /** + * Print R, a, b + * @param s: optional starting string + */ + void print(const std::string &s = "") const; + + /** + * Check if two lines are equal + * @param l2 - line to be compared + * @param tol : optional tolerance + * @return boolean - true if lines are equal + */ + bool equals(const Line3 &l2, double tol = 10e-9) const; + + /** + * Projecting a line to the image plane. Assumes this line is in camera frame. + * @param Dline: OptionalJacobian of projected line with respect to this line + * @return Unit3 - projected line in image plane, in homogenous coordinates. + * We use Unit3 since it is a manifold with the right dimension. + */ + Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; + + /** + * Returns point on the line that is at a certain distance starting from the + * point where the rotated XY axis intersects the line. + * @param distance (can be positive or negative) - positive is the positive + * direction of the rotated z axis that forms the line. The default value of zero + * returns the point where the rotated XY axis intersects the line. + * @return Point on the line + */ + Point3 point(double distance = 0) const; + + /** + * Transform a line from world to camera frame + * @param wTc - Pose3 of camera in world frame + * @param wL - Line3 in world frame + * @param Dpose - OptionalJacobian of transformed line with respect to p + * @param Dline - OptionalJacobian of transformed line with respect to l + * @return Transformed line in camera frame + */ + friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose, + OptionalJacobian<4, 4> Dline); +}; + +/** + * Transform a line from world to camera frame + * @param wTc - Pose3 of camera in world frame + * @param wL - Line3 in world frame + * @param Dpose - OptionalJacobian of transformed line with respect to p + * @param Dline - OptionalJacobian of transformed line with respect to l + * @return Transformed line in camera frame + */ +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose = boost::none, + OptionalJacobian<4, 4> Dline = boost::none); + +template<> +struct traits : public internal::Manifold {}; + +template<> +struct traits : public internal::Manifold {}; +} \ No newline at end of file diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c52127a452..9a80b937b2 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -325,7 +325,7 @@ class PinholeCamera: public PinholeBaseK { } 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 9359624230..79dbb9ce98 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -107,9 +107,9 @@ class PinholeBaseK: public PinholeBase { // 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 @@ class PinholeBaseK: public PinholeBase { } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholeBaseK @@ -425,7 +425,7 @@ class PinholePose: public PinholeBaseK { } 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 388318827d..2a1f108ca6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -317,7 +317,7 @@ class Pose2: public LieGroup { 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 31033a027f..1b9285100b 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; @@ -116,8 +118,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { - if (H) *H = ExpmapDerivative(xi); +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { + if (Hxi) *Hxi = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -125,8 +127,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Rot3 R = Rot3::Expmap(omega); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis + Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Vector3 omega_cross_v = omega.cross(v); // points towards axis Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { @@ -135,10 +137,10 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { - if (H) *H = LogmapDerivative(p); - const Vector3 w = Rot3::Logmap(p.rotation()); - const Vector3 T = p.translation(); +Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { + if (Hpose) *Hpose = LogmapDerivative(pose); + const Vector3 w = Rot3::Logmap(pose.rotation()); + const Vector3 T = pose.translation(); const double t = w.norm(); if (t < 1e-10) { Vector6 log; @@ -158,33 +160,33 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { } /* ************************************************************************* */ -Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { #ifdef GTSAM_POSE3_EXPMAP - return Expmap(xi, H); + return Expmap(xi, Hxi); #else Matrix3 DR; - Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); - if (H) { - *H = I_6x6; - H->topLeftCorner<3,3>() = DR; + Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0); + if (Hxi) { + *Hxi = I_6x6; + Hxi->topLeftCorner<3, 3>() = DR; } return Pose3(R, Point3(xi.tail<3>())); #endif } /* ************************************************************************* */ -Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #ifdef GTSAM_POSE3_EXPMAP - return Logmap(T, H); + return Logmap(pose, Hpose); #else Matrix3 DR; - Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); - if (H) { - *H = I_6x6; - H->topLeftCorner<3,3>() = DR; + Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0); + if (Hpose) { + *Hpose = I_6x6; + Hpose->topLeftCorner<3, 3>() = DR; } Vector6 xi; - xi << omega, T.translation(); + xi << omega, pose.translation(); return xi; #endif } @@ -261,16 +263,16 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { } /* ************************************************************************* */ -const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { - if (H) *H << Z_3x3, rotation().matrix(); +const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { + if (Hself) *Hself << Z_3x3, rotation().matrix(); return t_; } /* ************************************************************************* */ -const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const { - if (H) { - *H << I_3x3, Z_3x3; +const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { + if (Hself) { + *Hself << I_3x3, Z_3x3; } return R_; } @@ -284,9 +286,10 @@ Matrix4 Pose3::matrix() const { } /* ************************************************************************* */ -Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const { +Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; - return wTa * aTb; + return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ @@ -299,101 +302,101 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { #endif /* ************************************************************************* */ -Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1, - OptionalJacobian<6, 6> H2) const { - if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap(); - if (H2) *H2 = I_6x6; +Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, + OptionalJacobian<6, 6> HwTb) const { + if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); + if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; return wTa.inverse() * wTb; } /* ************************************************************************* */ -Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { +Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); - if (Dpose) { - Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->rightCols<3>() = R; + if (Hself) { + Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z()); + Hself->rightCols<3>() = R; } - if (Dpoint) { - *Dpoint = R; + if (Hpoint) { + *Hpoint = R; } - return R_ * p + t_; + return R_ * point + t_; } /* ************************************************************************* */ -Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { +Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_)); - if (Dpose) { + const Point3 q(Rt*(point - t_)); + if (Hself) { const double wx = q.x(), wy = q.y(), wz = q.z(); - (*Dpose) << + (*Hself) << 0.0, -wz, +wy,-1.0, 0.0, 0.0, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) { - *Dpoint = Rt; + if (Hpoint) { + *Hpoint = Rt; } return q; } /* ************************************************************************* */ -double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, - OptionalJacobian<1, 3> H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, + OptionalJacobian<1, 3> Hpoint) const { Matrix36 D_local_pose; Matrix3 D_local_point; - Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); - if (!H1 && !H2) { + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { return local.norm(); } else { Matrix13 D_r_local; const double r = norm3(local, D_r_local); - if (H1) *H1 = D_r_local * D_local_pose; - if (H2) *H2 = D_r_local * D_local_point; + if (Hself) *Hself = D_r_local * D_local_pose; + if (Hpoint) *Hpoint = D_r_local * D_local_point; return r; } } /* ************************************************************************* */ -double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1, - OptionalJacobian<1, 6> H2) const { +double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, + OptionalJacobian<1, 6> Hpose) const { Matrix13 D_local_point; - double r = range(pose.translation(), H1, H2 ? &D_local_point : 0); - if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); + double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); + if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); return r; } /* ************************************************************************* */ -Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, - OptionalJacobian<2, 3> H2) const { +Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself, + OptionalJacobian<2, 3> Hpoint) const { Matrix36 D_local_pose; Matrix3 D_local_point; - Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); - if (!H1 && !H2) { + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { return Unit3(local); } else { Matrix23 D_b_local; Unit3 b = Unit3::FromPoint3(local, D_b_local); - if (H1) *H1 = D_b_local * D_local_pose; - if (H2) *H2 = D_b_local * D_local_point; + if (Hself) *Hself = D_b_local * D_local_pose; + if (Hpoint) *Hpoint = D_b_local * D_local_point; return b; } } /* ************************************************************************* */ -Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1, - OptionalJacobian<2, 6> H2) const { - if (H2) { - H2->setZero(); - return bearing(pose.translation(), H1, H2.cols<3>(3)); +Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, + OptionalJacobian<2, 6> Hpose) const { + if (Hpose) { + Hpose->setZero(); + return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); } - return bearing(pose.translation(), H1, boost::none); + return bearing(pose.translation(), Hself, boost::none); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e90f91127a..3825b6241a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -44,7 +44,7 @@ class GTSAM_EXPORT Pose3: public LieGroup { private: Rot3 R_; ///< Rotation gRp, between global and pose frame - Point3 t_; ///< Translation gTp, from global origin to pose frame origin + Point3 t_; ///< Translation gPp, from global origin to pose frame origin public: @@ -75,8 +75,8 @@ class GTSAM_EXPORT Pose3: public LieGroup { /// 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { /// @{ /// 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { * 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { * 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { // 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { /** * @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 @@ class GTSAM_EXPORT Pose3: public LieGroup { /** 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { * 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { #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 @@ class GTSAM_EXPORT Pose3: public LieGroup { #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.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b0..01f62b8cb5 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -16,6 +16,7 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Varun Agrawal */ #include @@ -36,7 +37,6 @@ void Rot3::print(const std::string& s) const { /* ************************************************************************* */ Rot3 Rot3::Random(std::mt19937& rng) { - // TODO allow any engine without including all of boost :-( Unit3 axis = Unit3::Random(rng); uniform_real_distribution randomAngle(-M_PI, M_PI); double angle = randomAngle(rng); @@ -185,6 +185,12 @@ Vector Rot3::quaternion() const { return v; } +/* ************************************************************************* */ +pair Rot3::axisAngle() const { + const Vector3 omega = Rot3::Logmap(*this); + return std::pair(Unit3(omega), omega.norm()); +} + /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { return SO3::ExpmapDerivative(x); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4a..8f24f07c8d 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Richard Roberts * @author Luca Carlone + * @author Varun Agrawal */ // \callgraph @@ -194,22 +195,24 @@ namespace gtsam { /** * Convert from axis/angle representation - * @param axisw is the rotation axis, unit length - * @param angle rotation angle + * @param axis is the rotation axis, unit length + * @param angle rotation angle * @return incremental rotation */ static Rot3 AxisAngle(const Point3& axis, double angle) { + // Convert to unit vector. + Vector3 unitAxis = Unit3(axis).unitVector(); #ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, unitAxis)); #else - return Rot3(SO3::AxisAngle(axis,angle)); + return Rot3(SO3::AxisAngle(unitAxis,angle)); #endif } /** * Convert from axis/angle representation - * @param axis is the rotation axis - * @param angle rotation angle + * @param axis is the rotation axis + * @param angle rotation angle * @return incremental rotation */ static Rot3 AxisAngle(const Unit3& axis, double angle) { @@ -268,9 +271,13 @@ namespace gtsam { /// Syntatic sugar for composing two rotations Rot3 operator*(const Rot3& R2) const; - /// inverse of a rotation, TODO should be different for M/Q + /// inverse of a rotation Rot3 inverse() const { - return Rot3(Matrix3(transpose())); +#ifdef GTSAM_USE_QUATERNIONS + return Rot3(quaternion_.inverse()); +#else + return Rot3(rot_.matrix().transpose()); +#endif } /** @@ -406,7 +413,6 @@ namespace gtsam { * Return 3*3 transpose (inverse) rotation matrix */ Matrix3 transpose() const; - // TODO: const Eigen::Transpose transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -429,7 +435,7 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r) */ Vector3 rpy() const; @@ -439,7 +445,7 @@ namespace gtsam { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - inline double roll() const { return ypr()(2); } + inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations @@ -447,7 +453,7 @@ namespace gtsam { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - inline double pitch() const { return ypr()(1); } + inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations @@ -455,12 +461,22 @@ namespace gtsam { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - inline double yaw() const { return ypr()(0); } + inline double yaw() const { return xyz()(2); } /// @} /// @name Advanced Interface /// @{ + /** + * Compute the Euler axis and angle (in radians) representation + * of this rotation. + * The angle is in the range [0, Ï€]. If the angle is not in the range, + * the axis is flipped around accordingly so that the returned angle is + * within the specified range. + * @return pair consisting of Unit3 axis and angle in radians + */ + std::pair axisAngle() const; + /** Compute the quaternion representation of this rotation. * @return The quaternion */ @@ -528,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/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d38d33bf17..46a07e50ad 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,6 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -// TODO const Eigen::Transpose Rot3::transpose() const { Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 8af9a7144d..d609c289cf 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -19,8 +19,8 @@ #ifdef GTSAM_USE_QUATERNIONS -#include #include +#include #include using namespace std; @@ -79,9 +79,13 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: Could we do this? It works in Rot3M but not here, probably because - // here we create an intermediate value by calling matrix() - // const Eigen::Transpose Rot3::transpose() const { + // TODO: Maybe use return type `const Eigen::Transpose`? + // It works in Rot3M but not here, because of some weird behavior + // due to Eigen's expression templates which needs more investigation. + // For example, if we use matrix(), the value returned has a 1e-10, + // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary. + // Using eval() here doesn't help, it only helps if we use it in + // the downstream code. Matrix3 Rot3::transpose() const { return matrix().transpose(); } diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 19c016795d..c86b9b860a 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,13 +261,13 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (std::abs(tr + 1.0) < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-10) + if (tr + 1.0 < 1e-10) { + if (std::abs(R33 + 1.0) > 1e-5) omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-10) + else if (std::abs(R22 + 1.0) > 1e-5) omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); else - // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 9b6e4217ab..8a78bb83f0 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -33,9 +33,10 @@ using namespace std; namespace gtsam { +// TODO(frank): is this better than SOn::Random? // /* ************************************************************************* // */ static Vector3 randomOmega(boost::mt19937 &rng) { -// static boost::uniform_real randomAngle(-M_PI, M_PI); +// static std::uniform_real_distribution randomAngle(-M_PI, M_PI); // return Unit3::Random(rng).unitVector() * randomAngle(rng); // } @@ -58,9 +59,9 @@ Matrix4 SO4::Hat(const Vector6& xi) { Y(0, 1) = -xi(5); Y(0, 2) = +xi(4); Y(1, 2) = -xi(3); - Y(0, 3) = -xi(2); - Y(1, 3) = +xi(1); - Y(2, 3) = -xi(0); + Y(0, 3) = +xi(2); + Y(1, 3) = -xi(1); + Y(2, 3) = +xi(0); return Y - Y.transpose(); } @@ -72,9 +73,9 @@ Vector6 SO4::Vee(const Matrix4& X) { xi(5) = -X(0, 1); xi(4) = +X(0, 2); xi(3) = -X(1, 2); - xi(2) = -X(0, 3); - xi(1) = +X(1, 3); - xi(0) = -X(2, 3); + xi(2) = +X(0, 3); + xi(1) = -X(1, 3); + xi(0) = +X(2, 3); return xi; } @@ -208,9 +209,9 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) { if (H) { const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), q = R.topRightCorner<3, 1>(); - *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, // - Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, // - q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; + *H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2, // + Z_3x1, q, Z_3x1, m3, Z_3x1, -m1, // + -q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; } return M; } @@ -221,9 +222,9 @@ GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) { const Matrix43 M = R.leftCols<3>(); if (H) { const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3); - *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, // - Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, // - q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; + *H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2, // + Z_4x1, q, Z_4x1, m3, Z_4x1, -m1, // + -q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; } return M; } diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 344822c1f3..0d7f3e108d 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -20,6 +20,10 @@ #include +#include + +using namespace std; + namespace gtsam { // Implementation for N>5 just uses dynamic version @@ -109,4 +113,9 @@ typename SO::VectorN2 SO::vec( return X; } +template +void SO::print(const std::string& s) const { + cout << s << matrix_ << endl; +} + } // namespace gtsam diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp index be5acc23b2..37b6c17847 100644 --- a/gtsam/geometry/SOn.cpp +++ b/gtsam/geometry/SOn.cpp @@ -13,6 +13,7 @@ * @file SOn.cpp * @brief Definitions of dynamic specializations of SO(n) * @author Frank Dellaert + * @author Varun Agrawal * @date March 2019 */ @@ -37,11 +38,12 @@ Matrix SOn::Hat(const Vector& xi) { const size_t dmin = (n - 1) * (n - 2) / 2; X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); + // determine sign of last element (signs alternate) + double sign = pow(-1.0, xi.size()); // Now fill last row and column - double sign = 1.0; for (size_t i = 0; i < n - 1; i++) { const size_t j = n - 2 - i; - X(n - 1, j) = sign * xi(i); + X(n - 1, j) = -sign * xi(i); X(j, n - 1) = -X(n - 1, j); sign = -sign; } @@ -66,10 +68,10 @@ Vector SOn::Vee(const Matrix& X) { Vector xi(d); // Fill first n-1 spots from last row of X - double sign = 1.0; + double sign = pow(-1.0, xi.size()); for (size_t i = 0; i < n - 1; i++) { const size_t j = n - 2 - i; - xi(i) = sign * X(n - 1, j); + xi(i) = -sign * X(n - 1, j); sign = -sign; } diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 8b37f443ad..a6392c2f96 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -20,15 +20,15 @@ #include #include +#include #include - #include #include // TODO(frank): how to avoid? -#include #include #include #include +#include namespace gtsam { @@ -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 @@ -94,6 +94,16 @@ class SO : public LieGroup, internal::DimensionSO(N)> { return SO(R); } + /// Named constructor from lower dimensional matrix + template > + static SO Lift(size_t n, const Eigen::MatrixBase &R) { + Matrix Q = Matrix::Identity(n, n); + size_t p = R.rows(); + assert(p < n && R.cols() == p); + Q.topLeftCorner(p, p) = R; + return SO(Q); + } + /// Construct dynamic SO(n) from Fixed SO template > explicit SO(const SO& R) : matrix_(R.matrix()) {} @@ -148,9 +158,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /// @name Testable /// @{ - void print(const std::string& s) const { - std::cout << s << matrix_ << std::endl; - } + void print(const std::string& s = std::string()) const; bool equals(const SO& other, double tol) const { return equal_with_abs_tol(matrix_, other.matrix_, tol); @@ -210,11 +218,11 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * etc... For example, the vector-space isomorphic to so(5) is laid out as: * a b c d | u v w | x y | z * where the latter elements correspond to "telescoping" sub-algebras: - * 0 -z y -w d - * z 0 -x v -c - * -y x 0 -u b - * w -v u 0 -a - * -d c -b a 0 + * 0 -z y w -d + * z 0 -x -v c + * -y x 0 u -b + * -w v -u 0 a + * d -c b -a 0 * This scheme behaves exactly as expected for SO(2) and SO(3). */ static MatrixNN Hat(const TangentVector& xi); @@ -284,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; @@ -321,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.cpp b/gtsam/geometry/Unit3.cpp old mode 100755 new mode 100644 index 3d46b18b80..6f70d840e3 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -78,7 +78,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // NOTE(hayk): At some point it seemed like this reproducably resulted in // deadlock. However, I don't know why and I can no longer reproduce it. // It either was a red herring or there is still a latent bug left to debug. - tbb::mutex::scoped_lock lock(B_mutex_); + std::unique_lock lock(B_mutex_); #endif const bool cachedBasis = static_cast(B_); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 1c6945c4ca..27d41a014d 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -33,7 +33,7 @@ #include #ifdef GTSAM_USE_TBB -#include +#include // std::mutex #endif namespace gtsam { @@ -48,7 +48,7 @@ class Unit3 { mutable boost::optional H_B_; ///< Cached basis derivative #ifdef GTSAM_USE_TBB - mutable tbb::mutex B_mutex_; ///< Mutex to protect the cached basis. + mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis. #endif public: @@ -90,6 +90,8 @@ class Unit3 { /// Copy assignment Unit3& operator=(const Unit3 & u) { p_ = u.p_; + B_ = u.B_; + H_B_ = u.H_B_; return *this; } @@ -214,7 +216,7 @@ class Unit3 { /// @} 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 new file mode 100644 index 0000000000..9317fb737f --- /dev/null +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -0,0 +1,189 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3DFisheye.cpp + * @brief Unit tests for fisheye calibration class + * @author ghaggin + */ + +#include +#include +#include +#include + +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Fisheye) + +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 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 = 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; + const double td = + t * (1 + K.k1() * tt + K.k2() * t4 + K.k3() * t6 + K.k4() * t8); + Vector3 pd(td / r * xi, td / r * yi, 1); + Vector3 v = K.K() * pd; + + Point2 uv_sol(v[0] / v[2], v[1] / v[2]); + + Point2 uv = K.uncalibrate(kTestPoint2); + CHECK(assert_equal(uv, uv_sol)); +} + +/* ************************************************************************* */ +// 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)); +} + +/* ************************************************************************* */ +// 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 +/* +=========================================================== + +import numpy as np +import cv2 + +objpts = np.float64([[23,27,31]]).reshape(1,-1,3) + +cameraMatrix = np.float64([ + [250, 0, 320], + [0, 260, 240], + [0,0,1] +]) +alpha = 0.1/250 +distCoeffs = np.float64([-0.013721808247486035, 0.020727425669427896,-0.012786476702685545, 0.0025242267320687625]) + +rvec = np.float64([[0.,0.,0.]]) +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]]] + */ +// clang-format on +TEST(Cal3Fisheye, uncalibrate3) { + Point3 p3(23, 27, 31); + Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); + auto uv = K.uncalibrate(xi); + CHECK(assert_equal(uv, Point2(457.82638130304935, 408.18905848512986))); +} + +/* ************************************************************************* */ +TEST(Cal3Fisheye, calibrate1) { + Point2 pi; + Point2 uv; + Point2 pi_hat; + + pi = Point2(0.5, 0.5); // point in intrinsic coordinates + uv = K.uncalibrate(pi); // map intrinsic coord to image plane (pi) + pi_hat = K.calibrate(uv); // map image coords (pi) back to intrinsic coords + CHECK(traits::Equals(pi, pi_hat, + 1e-5)); // check that the inv mapping works + + pi = Point2(-0.7, -1.2); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK(traits::Equals(pi, pi_hat, 1e-5)); + + pi = Point2(-3, 5); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK(traits::Equals(pi, pi_hat, 1e-5)); + + pi = Point2(7, -12); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK(traits::Equals(pi, pi_hat, 1e-5)); +} + +/* ************************************************************************* */ +// 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) +TEST(Cal3Fisheye, calibrate3) { + Point3 p3(23, 27, 31); + Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); + Point2 uv(457.82638130304935, 408.18905848512986); + auto xi_hat = K.calibrate(uv); + CHECK(assert_equal(xi_hat, xi)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index c923e398ba..86a498cdc7 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) { Matrix actH1, actH2; try { bodyE.rotate(cRb, actH1, actH2); - } catch (exception e) { + } catch (exception& e) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp new file mode 100644 index 0000000000..9ed12aef7d --- /dev/null +++ b/gtsam/geometry/tests/testLine3.cpp @@ -0,0 +1,155 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Line3) +GTSAM_CONCEPT_MANIFOLD_INST(Line3) + +static const Line3 l(Rot3(), 1, 1); + +// Testing equals function of Line3 +TEST(Line3, equals) { + Line3 l_same = l; + EXPECT(l.equals(l_same)); + Line3 l2(Rot3(), 1, 2); + EXPECT(!l.equals(l2)); +} + +// testing localCoordinates along 4 dimensions +TEST(Line3, localCoordinates) { + // l1 and l differ only in a_ + Line3 l1(Rot3(), 2, 1); + Vector4 v1(0, 0, -1, 0); + CHECK(assert_equal(l1.localCoordinates(l), v1)); + + // l2 and l differ only in b_ + Line3 l2(Rot3(), 1, 2); + Vector4 v2(0, 0, 0, -1); + CHECK(assert_equal(l2.localCoordinates(l), v2)); + + // l3 and l differ in R_x + Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0)); + Line3 l3(r3, 1, 1); + Vector4 v3(-M_PI / 4, 0, 0, 0); + CHECK(assert_equal(l3.localCoordinates(l), v3)); + + // l4 and l differ in R_y + Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0)); + Line3 l4(r4, 1, 1); + Vector4 v4(0, -M_PI / 4, 0, 0); + CHECK(assert_equal(l4.localCoordinates(l), v4)); + + // Jacobians + Line3 l5(Rot3::Expmap(Vector3(M_PI / 3, -M_PI / 4, 0)), -0.8, 2); + Values val; + val.insert(1, l); + val.insert(2, l5); + Line3_ l_(1); + Line3_ l5_(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + Vector4_ local_(l5_, &Line3::localCoordinates, l_); + ExpressionFactor f(model, l5.localCoordinates(l), local_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +// testing retract along 4 dimensions +TEST(Line3, retract) { + // l1 and l differ only in a_ + Vector4 v1(0, 0, 0, 1); + Line3 l1(Rot3(), 1, 2); + EXPECT(l1.equals(l.retract(v1))); + + // l2 and l differ only in b_ + Vector4 v2(0, 0, 1, 0); + Line3 l2(Rot3(), 2, 1); + EXPECT(l2.equals(l.retract(v2))); + + // l3 and l differ in R_x + Vector4 v3(M_PI / 4, 0, 0, 0); + Rot3 r3; + r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0)); + Line3 l3(r3, 1, 1); + EXPECT(l3.equals(l.retract(v3))); + + // l4 and l differ in R_y + Vector4 v4(0, M_PI / 4, 0, 0); + Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0)); + Line3 l4(r4, 1, 1); + EXPECT(l4.equals(l.retract(v4))); + + // Jacobians + Vector4 v5(M_PI / 3, -M_PI / 4, -0.4, 1.2); // arbitrary vector + Values val; + val.insert(1, l); + val.insert(2, v5); + Line3_ l_(1); + Vector4_ v5_(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + Line3_ retract_(l_, &Line3::retract, v5_); + ExpressionFactor f(model, l.retract(v5), retract_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +// testing manifold property - Retract(p, Local(p,q)) == q +TEST(Line3, retractOfLocalCoordinates) { + Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0)); + Line3 l2(r2, 5, 9); + EXPECT(assert_equal(l.retract(l.localCoordinates(l2)), l2)); +} + +// testing manifold property - Local(p, Retract(p, v)) == v +TEST(Line3, localCoordinatesOfRetract) { + Vector4 r2(2.3, 0.987, -3, 4); + EXPECT(assert_equal(l.localCoordinates(l.retract(r2)), r2)); +} + +// transform from world to camera test +TEST(Line3, transformToExpressionJacobians) { + Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0)); + Vector3 t(0, 0, 0); + Pose3 p(r, t); + + Line3 l_c(r.inverse(), 1, 1); + Line3 l_w(Rot3(), 1, 1); + EXPECT(l_c.equals(transformTo(p, l_w))); + + Line3_ l_(1); + Pose3_ p_(2); + Values val; + val.insert(1, l_w); + val.insert(2, p); + + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + ExpressionFactor f(model, transformTo(p, l_w), transformTo(p_, l_)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +// projection in camera frame test +TEST(Line3, projection) { + Rot3 r = Rot3::Expmap(Vector3(0, 0, 0)); + Line3 wL(r, 1, 1); + + Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0)); + EXPECT(expected.equals(wL.project())); + + Values val; + val.insert(1, wL); + Line3_ wL_(1); + + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1); + Unit3_ projected_(wL_, &Line3::project); + ExpressionFactor f(model, expected, projected_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 54cf84ea87..dc5851319b 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -66,8 +66,8 @@ TEST (OrientedPlane3, transform) { OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose, none, none); OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none); - EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9)); - EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9)); + EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5)); + EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; @@ -75,18 +75,18 @@ TEST (OrientedPlane3, transform) { // because the Transform class uses a wrong order of Jacobians in interface OrientedPlane3::Transform(plane, pose, actualH1, none); expectedH1 = numericalDerivative22(Transform_, plane, pose); - EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); OrientedPlane3::Transform(plane, pose, none, actualH2); expectedH2 = numericalDerivative21(Transform_, plane, pose); - EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } { plane.transform(pose, actualH1, none); expectedH1 = numericalDerivative21(transform_, plane, pose); - EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); plane.transform(pose, none, actualH2); expectedH2 = numericalDerivative22(Transform_, plane, pose); - EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } } @@ -157,8 +157,8 @@ TEST (OrientedPlane3, error2) { boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); - EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); - EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } //******************************************************************************* @@ -171,19 +171,19 @@ TEST (OrientedPlane3, jacobian_retract) { Vector3 v (-0.1, 0.2, 0.3); plane.retract(v, H_actual); Matrix H_expected_numerical = numericalDerivative11(f, v); - EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5)); } { Vector3 v (0, 0, 0); plane.retract(v, H_actual); Matrix H_expected_numerical = numericalDerivative11(f, v); - EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5)); } } /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8433bbb01d..a169c833cb 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)); @@ -649,8 +672,8 @@ TEST(Pose3, Bearing) { // Check numerical derivatives expectedH1 = numericalDerivative21(bearing_proxy, x1, l1); expectedH2 = numericalDerivative22(bearing_proxy, x1, l1); - EXPECT(assert_equal(expectedH1, actualH1)); - EXPECT(assert_equal(expectedH2, actualH2)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } TEST(Pose3, Bearing2) { @@ -660,8 +683,8 @@ TEST(Pose3, Bearing2) { // Check numerical derivatives expectedH1 = numericalDerivative21(bearing_proxy, x2, l4); expectedH2 = numericalDerivative22(bearing_proxy, x2, l4); - EXPECT(assert_equal(expectedH1, actualH1)); - EXPECT(assert_equal(expectedH2, actualH2)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } TEST(Pose3, PoseToPoseBearing) { @@ -681,8 +704,8 @@ TEST(Pose3, PoseToPoseBearing) { expectedH2.setZero(); expectedH2.block<2, 3>(0, 3) = H2block; - EXPECT(assert_equal(expectedH1, actualH1)); - EXPECT(assert_equal(expectedH2, actualH2)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ @@ -1008,13 +1031,20 @@ TEST(Pose3, print) { // Generate the expected output std::stringstream expected; Point3 translation(1, 2, 3); + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + expected << "1\n" + "2\n" + "3;\n"; +#else expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';"; +#endif // reset cout to the original stream std::cout.rdbuf(oldbuf); // Get substring corresponding to translation part - std::string actual = redirectStream.str().substr(47, 11); + std::string actual = redirectStream.str().substr(38, 11); CHECK_EQUAL(expected.str(), actual); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c95b85f217..598c57b24c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,6 +14,7 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -115,6 +116,10 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual,1e-5)); Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); CHECK(assert_equal(expected,actual2,1e-5)); + + axis = Vector3(0, 50, 0); + Rot3 actual3 = Rot3::AxisAngle(axis, angle); + CHECK(assert_equal(expected,actual3,1e-5)); } /* ************************************************************************* */ @@ -176,63 +181,81 @@ TEST( Rot3, retract) } /* ************************************************************************* */ -TEST(Rot3, log) -{ +TEST(Rot3, log) { static const double PI = boost::math::constants::pi(); Vector w; Rot3 R; -#define CHECK_OMEGA(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ - R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); +#define CHECK_OMEGA(X, Y, Z) \ + w = (Vector(3) << X, Y, Z).finished(); \ + R = Rot3::Rodrigues(w); \ + EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); // Check zero - CHECK_OMEGA( 0, 0, 0) + CHECK_OMEGA(0, 0, 0) // create a random direction: - double norm=sqrt(1.0+16.0+4.0); - double x=1.0/norm, y=4.0/norm, z=2.0/norm; + double norm = sqrt(1.0 + 16.0 + 4.0); + double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm; // Check very small rotation for Taylor expansion // Note that tolerance above is 1e-12, so Taylor is pretty good ! double d = 0.0001; - CHECK_OMEGA( d, 0, 0) - CHECK_OMEGA( 0, d, 0) - CHECK_OMEGA( 0, 0, d) - CHECK_OMEGA(x*d, y*d, z*d) + CHECK_OMEGA(d, 0, 0) + CHECK_OMEGA(0, d, 0) + CHECK_OMEGA(0, 0, d) + CHECK_OMEGA(x * d, y * d, z * d) // check normal rotation d = 0.1; - CHECK_OMEGA( d, 0, 0) - CHECK_OMEGA( 0, d, 0) - CHECK_OMEGA( 0, 0, d) - CHECK_OMEGA(x*d, y*d, z*d) + CHECK_OMEGA(d, 0, 0) + CHECK_OMEGA(0, d, 0) + CHECK_OMEGA(0, 0, d) + CHECK_OMEGA(x * d, y * d, z * d) // Check 180 degree rotations - CHECK_OMEGA( PI, 0, 0) - CHECK_OMEGA( 0, PI, 0) - CHECK_OMEGA( 0, 0, PI) + CHECK_OMEGA(PI, 0, 0) + CHECK_OMEGA(0, PI, 0) + CHECK_OMEGA(0, 0, PI) // Windows and Linux have flipped sign in quaternion mode -#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) - w = (Vector(3) << x*PI, y*PI, z*PI).finished(); +#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) + w = (Vector(3) << x * PI, y * PI, z * PI).finished(); R = Rot3::Rodrigues(w); - EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); + EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); #else - CHECK_OMEGA(x*PI,y*PI,z*PI) + CHECK_OMEGA(x * PI, y * PI, z * PI) #endif // Check 360 degree rotations -#define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ - R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); - - CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) - CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) - CHECK_OMEGA_ZERO( 0, 0, 2.0*PI) - CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) +#define CHECK_OMEGA_ZERO(X, Y, Z) \ + w = (Vector(3) << X, Y, Z).finished(); \ + R = Rot3::Rodrigues(w); \ + EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); + + CHECK_OMEGA_ZERO(2.0 * PI, 0, 0) + CHECK_OMEGA_ZERO(0, 2.0 * PI, 0) + CHECK_OMEGA_ZERO(0, 0, 2.0 * PI) + CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI) + + // Check problematic case from Lund dataset vercingetorix.g2o + // This is an almost rotation with determinant not *quite* 1. + Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, // + -0.03997006, -0.88835923, 0.45740671, // + -0.16293753, 0.45743998, 0.87418537); + + // Rot3's Logmap returns different, but equivalent compacted + // axis-angle vectors depending on whether Rot3 is implemented + // by Quaternions or SO3. + #if defined(GTSAM_USE_QUATERNIONS) + // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); + #else + // SO3 does not bound angle resulting in ~180.1 degrees + EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), + (Vector)Rot3::Logmap(Rlund), 1e-8)); + #endif } /* ************************************************************************* */ @@ -376,7 +399,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); + CHECK(assert_equal(actual.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); @@ -531,16 +554,15 @@ TEST( Rot3, logmapStability ) { TEST(Rot3, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << - 0.271018623057411, 0.278786459830371, 0.921318086098018, - 0.578529366719085, 0.717799701969298, -0.387385285854279, - -0.769319620053772, 0.637998195662053, 0.033250932803219).finished()); + Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018, + 0.578529366719085, 0.717799701969298, -0.387385285854279, + -0.769319620053772, 0.637998195662053, 0.033250932803219); - Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << - -0.207341903877828, 0.250149415542075, 0.945745528564780, - 0.881304914479026, -0.371869043667957, 0.291573424846290, - 0.424630407073532, 0.893945571198514, -0.143353873763946).finished()); + Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, + 0.599136268678053); + Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780, + 0.881304914479026, -0.371869043667957, 0.291573424846290, + 0.424630407073532, 0.893945571198514, -0.143353873763946); // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); @@ -661,6 +683,65 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } +/* ************************************************************************* */ +TEST(Rot3, axisAngle) { + Unit3 actualAxis; + double actualAngle; + +// not a lambda as otherwise we can't trace error easily +#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \ + std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \ + EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \ + EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \ + EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle))) + + // CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2) + Vector3 omega(0.1, 0.4, 0.2); + Unit3 axis(omega), _axis(-omega); + CHECK_AXIS_ANGLE(axis, omega.norm(), R); + + // rotate by 90 + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0)) + CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2)) + + // rotate by -90 + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0)) + CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2)) + + // rotate by 270 + const double theta270 = M_PI + M_PI / 2; + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0)) + CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270)) + + // rotate by -270 + const double theta_270 = -(M_PI + M_PI / 2); // 90 (or -270) degrees + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0)) + CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270)) + + const double theta195 = 195 * M_PI / 180; + const double theta165 = 165 * M_PI / 180; + + /// Non-trivial angle 165 + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0)) + CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165)) + + /// Non-trivial angle 195 + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0)) + CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195)) +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index eb84a4d555..1cf8caed20 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -46,7 +46,7 @@ TEST(SOn, SO0) { EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(0, R.dim()); - EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); + EXPECT_LONGS_EQUAL(0, traits::GetDimension(R)); } //****************************************************************************** @@ -56,7 +56,7 @@ TEST(SOn, SO5) { EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(10, R.dim()); - EXPECT_LONGS_EQUAL(-1, traits::GetDimension(R)); + EXPECT_LONGS_EQUAL(10, traits::GetDimension(R)); } //****************************************************************************** @@ -95,32 +95,42 @@ TEST(SOn, Random) { //****************************************************************************** TEST(SOn, HatVee) { - Vector6 v; - v << 1, 2, 3, 4, 5, 6; + Vector10 v; + v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; Matrix expected2(2, 2); expected2 << 0, -1, 1, 0; const auto actual2 = SOn::Hat(v.head<1>()); - CHECK(assert_equal(expected2, actual2)); - CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); Matrix expected3(3, 3); - expected3 << 0, -3, 2, // - 3, 0, -1, // - -2, 1, 0; + expected3 << 0, -3, 2, // + 3, 0, -1, // + -2, 1, 0; const auto actual3 = SOn::Hat(v.head<3>()); - CHECK(assert_equal(expected3, actual3)); - CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); - CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); + EXPECT(assert_equal(expected3, actual3)); + EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3)); + EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); Matrix expected4(4, 4); - expected4 << 0, -6, 5, -3, // - 6, 0, -4, 2, // - -5, 4, 0, -1, // - 3, -2, 1, 0; - const auto actual4 = SOn::Hat(v); - CHECK(assert_equal(expected4, actual4)); - CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); + expected4 << 0, -6, 5, 3, // + 6, 0, -4, -2, // + -5, 4, 0, 1, // + -3, 2, -1, 0; + const auto actual4 = SOn::Hat(v.head<6>()); + EXPECT(assert_equal(expected4, actual4)); + EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4))); + + Matrix expected5(5, 5); + expected5 << 0,-10, 9, 7, -4, // + 10, 0, -8, -6, 3, // + -9, 8, 0, 5, -2, // + -7, 6, -5, 0, 1, // + 4, -3, 2, -1, 0; + const auto actual5 = SOn::Hat(v); + EXPECT(assert_equal(expected5, actual5)); + EXPECT(assert_equal((Vector)v, SOn::Vee(actual5))); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 97412f94d1..4f71a48dad 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -151,7 +151,7 @@ TEST( triangulation, fourPoses) { // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - SimpleCamera camera3(pose3, *sharedCal); + PinholeCamera camera3(pose3, *sharedCal); Point2 z3 = camera3.project(landmark); poses += pose3; @@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) { // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera camera4(pose4, *sharedCal); + PinholeCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); @@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) { TEST( triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera camera1(pose1, K1); + PinholeCamera camera1(pose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - SimpleCamera camera2(pose2, K2); + PinholeCamera camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet cameras; + CameraSet > cameras; Point2Vector measurements; cameras += camera1, camera2; @@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); - SimpleCamera camera3(pose3, K3); + PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); cameras += camera3; @@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Cal3_S2 K4(700, 500, 0, 640, 480); - SimpleCamera camera4(pose4, K4); + PinholeCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); @@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) { TEST( triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera camera1(pose1, K1); + PinholeCamera camera1(pose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - SimpleCamera camera2(pose2, K2); + PinholeCamera camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet cameras; + CameraSet > cameras; Point2Vector measurements; cameras += camera1, camera2; @@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) { // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); - SimpleCamera camera3(pose3, K3); + PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); cameras += camera3; @@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) { //****************************************************************************** TEST( triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera camera1(pose1, *sharedCal); + PinholeCamera camera1(pose1, *sharedCal); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); @@ -382,8 +382,8 @@ TEST( triangulation, StereotriangulateNonlinear ) { graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); - graph.emplace_shared >(Symbol('x',2), Pose3(m2), posePrior); + graph.addPrior(Symbol('x',1), Pose3(m1), posePrior); + graph.addPrior(Symbol('x',2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 59b99e525c..ddf60a2568 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -27,7 +27,6 @@ #include #include #include -#include #include @@ -59,8 +58,8 @@ TEST(Unit3, point3) { for(Point3 p: ps) { Unit3 s(p); expectedH = numericalDerivative11(point3_, s); - EXPECT(assert_equal(p, s.point3(actualH), 1e-8)); - EXPECT(assert_equal(expectedH, actualH, 1e-9)); + EXPECT(assert_equal(p, s.point3(actualH), 1e-5)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -74,18 +73,18 @@ TEST(Unit3, rotate) { Unit3 p(1, 0, 0); Unit3 expected = Unit3(R.column(1)); Unit3 actual = R * p; - EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-5)); Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(rotate_, R, p); R.rotate(p, actualH, boost::none); - EXPECT(assert_equal(expectedH, actualH, 1e-9)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(rotate_, R, p); R.rotate(p, boost::none, actualH); - EXPECT(assert_equal(expectedH, actualH, 1e-9)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -99,19 +98,19 @@ TEST(Unit3, unrotate) { Unit3 p(1, 0, 0); Unit3 expected = Unit3(1, 1, 0); Unit3 actual = R.unrotate(p); - EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-5)); Matrix actualH, expectedH; // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(unrotate_, R, p); R.unrotate(p, actualH, boost::none); - EXPECT(assert_equal(expectedH, actualH, 1e-9)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(unrotate_, R, p); R.unrotate(p, boost::none, actualH); - EXPECT(assert_equal(expectedH, actualH, 1e-9)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -120,7 +119,7 @@ TEST(Unit3, dot) { Unit3 q = p.retract(Vector2(0.5, 0)); Unit3 r = p.retract(Vector2(0.8, 0)); Unit3 t = p.retract(Vector2(0, 0.3)); - EXPECT(assert_equal(1.0, p.dot(p), 1e-8)); + EXPECT(assert_equal(1.0, p.dot(p), 1e-5)); EXPECT(assert_equal(0.877583, p.dot(q), 1e-5)); EXPECT(assert_equal(0.696707, p.dot(r), 1e-5)); EXPECT(assert_equal(0.955336, p.dot(t), 1e-5)); @@ -131,18 +130,18 @@ TEST(Unit3, dot) { boost::none, boost::none); { p.dot(q, H1, H2); - EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); - EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-5)); } { p.dot(r, H1, H2); - EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); - EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-5)); } { p.dot(t, H1, H2); - EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); - EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); + EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-5)); + EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-5)); } } @@ -150,7 +149,7 @@ TEST(Unit3, dot) { TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // r = p.retract(Vector2(0.8, 0)); - EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); @@ -160,13 +159,13 @@ TEST(Unit3, error) { expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } } @@ -177,7 +176,7 @@ TEST(Unit3, error2) { Unit3 r = p.retract(Vector2(0.8, 0)); // Hard-coded as simple regression values - EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5)); EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5)); @@ -187,25 +186,25 @@ TEST(Unit3, error2) { expected = numericalDerivative21( boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); p.errorVector(q, actual, boost::none); - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative21( boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); p.errorVector(r, actual, boost::none); - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); p.errorVector(q, boost::none, actual); - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); p.errorVector(r, boost::none, actual); - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal(expected, actual, 1e-5)); } } @@ -213,9 +212,9 @@ TEST(Unit3, error2) { TEST(Unit3, distance) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // r = p.retract(Vector2(0.8, 0)); - EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); - EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8); - EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8); + EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-5); + EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-5); + EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-5); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian @@ -223,13 +222,13 @@ TEST(Unit3, distance) { expected = numericalGradient( boost::bind(&Unit3::distance, &p, _1, boost::none), q); p.distance(q, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalGradient( boost::bind(&Unit3::distance, &p, _1, boost::none), r); p.distance(r, actual); - EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); + EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } } @@ -237,7 +236,7 @@ TEST(Unit3, distance) { TEST(Unit3, localCoordinates0) { Unit3 p; Vector actual = p.localCoordinates(p); - EXPECT(assert_equal(Z_2x1, actual, 1e-8)); + EXPECT(assert_equal(Z_2x1, actual, 1e-5)); } TEST(Unit3, localCoordinates) { @@ -245,46 +244,46 @@ TEST(Unit3, localCoordinates) { Unit3 p, q; Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5)); + EXPECT(assert_equal(q, p.retract(expected), 1e-5)); } { Unit3 p, q(1, 6.12385e-21, 0); Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5)); + EXPECT(assert_equal(q, p.retract(expected), 1e-5)); } { Unit3 p, q(-1, 0, 0); Vector2 expected(M_PI, 0); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-5)); + EXPECT(assert_equal(q, p.retract(expected), 1e-5)); } { Unit3 p, q(0, 1, 0); Vector2 expected(0,-M_PI_2); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-5)); + EXPECT(assert_equal(q, p.retract(expected), 1e-5)); } { Unit3 p, q(0, -1, 0); Vector2 expected(0, M_PI_2); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(q, p.retract(expected), 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-5)); + EXPECT(assert_equal(q, p.retract(expected), 1e-5)); } { Unit3 p(0,1,0), q(0,-1,0); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + EXPECT(assert_equal(q, p.retract(actual), 1e-5)); } { Unit3 p(0,0,1), q(0,0,-1); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + EXPECT(assert_equal(q, p.retract(actual), 1e-5)); } double twist = 1e-4; @@ -329,11 +328,11 @@ TEST(Unit3, basis) { // with H, first time EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); // with H, cached EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } //******************************************************************************* @@ -349,7 +348,7 @@ TEST(Unit3, basis_derivatives) { Matrix62 expectedH = numericalDerivative11( boost::bind(BasisTest, _1, boost::none), p); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -361,14 +360,14 @@ TEST(Unit3, retract) { Unit3 expected(0.877583, 0, 0.479426); Unit3 actual = p.retract(v); EXPECT(assert_equal(expected, actual, 1e-6)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5)); } { Unit3 p; Vector2 v(0, 0); Unit3 actual = p.retract(v); EXPECT(assert_equal(p, actual, 1e-6)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5)); } } @@ -382,13 +381,13 @@ TEST (Unit3, jacobian_retract) { Vector2 v (-0.2, 0.1); p.retract(v, H); Matrix H_expected_numerical = numericalDerivative11(f, v); - EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + EXPECT(assert_equal(H_expected_numerical, H, 1e-5)); } { Vector2 v (0, 0); p.retract(v, H); Matrix H_expected_numerical = numericalDerivative11(f, v); - EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + EXPECT(assert_equal(H_expected_numerical, H, 1e-5)); } } @@ -398,8 +397,8 @@ TEST(Unit3, retract_expmap) { Vector2 v((M_PI / 2.0), 0); Unit3 expected(Point3(0, 0, 1)); Unit3 actual = p.retract(v); - EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(expected, actual, 1e-5)); + EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5)); } //******************************************************************************* @@ -429,7 +428,7 @@ TEST(Unit3, localCoordinates_retract) { // Check if the local coordinates and retract return consistent results. Vector v12 = s1.localCoordinates(s2); Unit3 actual_s2 = s1.retract(v12); - EXPECT(assert_equal(s2, actual_s2, 1e-9)); + EXPECT(assert_equal(s2, actual_s2, 1e-5)); } } @@ -438,10 +437,10 @@ TEST (Unit3, FromPoint3) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point Unit3 expected(point); - EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8)); + EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); Matrix expectedH = numericalDerivative11( boost::bind(Unit3::FromPoint3, _1, boost::none), point); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } //******************************************************************************* @@ -456,7 +455,7 @@ TEST(Unit3, ErrorBetweenFactor) { // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); for (size_t i = 0; i < data.size(); i++) { - graph.emplace_shared >(U(i), data[i], R_prior); + graph.addPrior(U(i), data[i], R_prior); } // Add process factors using the dot product error function. @@ -485,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); @@ -495,7 +503,7 @@ TEST(actualH, Serialization) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index ed61c75b59..8cdf0fdc05 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -124,7 +123,17 @@ std::pair triangulationGraph( return std::make_pair(graph, values); } -/// PinholeCamera specific version // TODO: (chris) why does this exist? +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +/// DEPRECATED: PinholeCamera specific version +template +Point3 triangulateNonlinear( + const CameraSet >& cameras, + const Point2Vector& measurements, const Point3& initialEstimate) { + return triangulateNonlinear > // + (cameras, measurements, initialEstimate); +} + +/// DEPRECATED: PinholeCamera specific version template std::pair triangulationGraph( const CameraSet >& cameras, @@ -133,6 +142,7 @@ std::pair triangulationGraph( return triangulationGraph > // (cameras, measurements, landmarkKey, initialEstimate); } +#endif /** * Optimize for triangulation @@ -187,15 +197,6 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } -/// PinholeCamera specific version // TODO: (chris) why does this exist? -template -Point3 triangulateNonlinear( - const CameraSet >& cameras, - const Point2Vector& measurements, const Point3& initialEstimate) { - return triangulateNonlinear > // - (cameras, measurements, initialEstimate); -} - /** * Create a 3*4 camera projection matrix from calibration and pose. * Functor for partial application on calibration @@ -214,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 a69fb9b8c5..0597ece98f 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/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 639bcbab00..5b53a57193 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -125,7 +125,7 @@ namespace gtsam { void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { for(Key j: clique->conditional()->frontals()) nodes_[j] = clique; - if (parent_clique != NULL) { + if (parent_clique != nullptr) { clique->parent_ = parent_clique; parent_clique->children.push_back(clique); } else { @@ -430,7 +430,7 @@ namespace gtsam { template void BayesTree::removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans) { - // base case is NULL, if so we do nothing and return empties above + // base case is nullptr, if so we do nothing and return empties above if (clique) { // remove the clique from orphans in case it has been added earlier orphans->remove(clique); diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index e762786f5e..a02fe274ef 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 1d486030c5..295122879e 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/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 34ca8ab7fd..df68019e12 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -55,8 +55,8 @@ bool FactorGraph::equals(const This& fg, double tol) const { // check whether the factors are the same, in same order. for (size_t i = 0; i < factors_.size(); i++) { sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; + if (f1 == nullptr && f2 == nullptr) continue; + if (f1 == nullptr || f2 == nullptr) return false; if (!f1->equals(*f2, tol)) return false; } return true; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 600e3f9ed2..2bc9578b24 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -353,7 +353,7 @@ class FactorGraph { */ void resize(size_t size) { factors_.resize(size); } - /** delete factor without re-arranging indexes by inserting a NULL pointer + /** delete factor without re-arranging indexes by inserting a nullptr pointer */ void remove(size_t i) { factors_[i].reset(); } diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 0875755aa6..266c54ca61 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, assert((size_t)count == variableIndex.nEntries()); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */ double knobs[CCOLAMD_KNOBS]; ccolamd_set_defaults(knobs); knobs[CCOLAMD_DENSE_ROW] = -1; @@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) { int outputError; - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0], &iperm[0]); Ordering result; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 9da7a16091..3f6d69e917 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -143,7 +143,7 @@ namespace gtsam { * allocateVectorValues */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index fcc73f80e3..b6f5acd520 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -106,7 +106,7 @@ namespace gtsam { * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp new file mode 100644 index 0000000000..0802deff4e --- /dev/null +++ b/gtsam/linear/GaussianFactor.cpp @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianFactor.cpp + * @brief A factor with a quadratic error function - a Gaussian + * @brief GaussianFactor + * @author Fan Jiang + */ + +// \callgraph + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ + VectorValues GaussianFactor::hessianDiagonal() const { + VectorValues d; + hessianDiagonalAdd(d); + return d; + } + +} diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index a80b4dfd40..9b4c5f940b 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -100,7 +100,10 @@ namespace gtsam { virtual Matrix information() const = 0; /// Return the diagonal of the Hessian for this factor - virtual VectorValues hessianDiagonal() const = 0; + VectorValues hessianDiagonal() const; + + /// Add the current diagonal to a VectorValues instance + virtual void hessianDiagonalAdd(VectorValues& d) const = 0; /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const = 0; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 8dc3600c68..69890c32d6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -255,8 +255,7 @@ namespace gtsam { VectorValues d; for (const sharedFactor& factor : *this) { if(factor){ - VectorValues di = factor->hessianDiagonal(); - d.addInPlace_(di); + factor->hessianDiagonalAdd(d); } } return d; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cc82e2fa0b..7bf65353b8 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -302,12 +302,14 @@ Matrix HessianFactor::information() const { } /* ************************************************************************* */ -VectorValues HessianFactor::hessianDiagonal() const { - VectorValues d; +void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - d.emplace(keys_[j], info_.diagonal(j)); + auto result = d.emplace(keys_[j], info_.diagonal(j)); + if(!result.second) { + // if emplace fails, it returns an iterator to the existing element, which we add to: + result.first->second += info_.diagonal(j); + } } - return d; } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d1c362a546..64b7640871 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -293,8 +293,11 @@ namespace gtsam { */ Matrix information() const override; - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const override; + /// Add the current diagonal to a VectorValues instance + void hessianDiagonalAdd(VectorValues& d) const override; + + /// Using the base method + using Base::hessianDiagonal; /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* d) const override; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2310d88f04..b8a08be9e3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -515,7 +515,7 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { double JacobianFactor::error(const VectorValues& c) const { Vector e = unweighted_error(c); // Use the noise model distance function to get the correct error if available. - if (model_) return 0.5 * model_->distance(e); + if (model_) return 0.5 * model_->squaredMahalanobisDistance(e); return 0.5 * e.dot(e); } @@ -542,21 +542,31 @@ Matrix JacobianFactor::information() const { } /* ************************************************************************* */ -VectorValues JacobianFactor::hessianDiagonal() const { - VectorValues d; +void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; size_t nj = Ab_(pos).cols(); - Vector dj(nj); + auto result = d.emplace(j, nj); + + Vector& dj = result.first->second; + for (size_t k = 0; k < nj; ++k) { - Vector column_k = Ab_(pos).col(k); - if (model_) - model_->whitenInPlace(column_k); - dj(k) = dot(column_k, column_k); + Eigen::Ref column_k = Ab_(pos).col(k); + if (model_) { + Vector column_k_copy = column_k; + model_->whitenInPlace(column_k_copy); + if(!result.second) + dj(k) += dot(column_k_copy, column_k_copy); + else + dj(k) = dot(column_k_copy, column_k_copy); + } else { + if (!result.second) + dj(k) += dot(column_k, column_k); + else + dj(k) = dot(column_k, column_k); + } } - d.emplace(j, dj); } - return d; } /* ************************************************************************* */ @@ -840,7 +850,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta if (!model_) { throw std::invalid_argument( - "JacobianFactor::splitConditional cannot be given a NULL noise model"); + "JacobianFactor::splitConditional cannot be given a nullptr noise model"); } if (nrFrontals > size()) { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index fad05a7295..49c47c7f02 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -215,8 +215,11 @@ namespace gtsam { */ Matrix information() const override; - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const override; + /// Using the base method + using Base::hessianDiagonal; + + /// Add the current diagonal to a VectorValues instance + void hessianDiagonalAdd(VectorValues& d) const override; /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* d) const override; @@ -398,7 +401,7 @@ namespace gtsam { * @param keys in some order * @param diemnsions of the variables in same order * @param m output dimension - * @param model noise model (default NULL) + * @param model noise model (default nullptr) */ template JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2cd..bf799a2ba7 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)); @@ -153,7 +153,7 @@ void Fair::print(const std::string &s="") const bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_ ) < tol; } @@ -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)); } @@ -190,7 +190,7 @@ void Huber::print(const std::string &s="") const { bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(k_ - p->k_) < tol; } @@ -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; } @@ -223,7 +223,7 @@ void Cauchy::print(const std::string &s="") const { bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(ksquared_ - p->ksquared_) < tol; } @@ -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 { @@ -266,7 +266,7 @@ void Tukey::print(const std::string &s="") const { bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -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); } @@ -296,7 +296,7 @@ void Welsch::print(const std::string &s="") const { bool Welsch::equals(const Base &expected, double tol) const { const Welsch* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -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); } @@ -330,7 +330,7 @@ void GemanMcClure::print(const std::string &s="") const { bool GemanMcClure::equals(const Base &expected, double tol) const { const GemanMcClure* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -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_; @@ -372,7 +372,7 @@ void DCS::print(const std::string &s="") const { bool DCS::equals(const Base &expected, double tol) const { const DCS* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -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); } @@ -411,7 +411,7 @@ void L2WithDeadZone::print(const std::string &s="") const { bool L2WithDeadZone::equals(const Base &expected, double tol) const { const L2WithDeadZone* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(k_ - p->k_) < tol; } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1f7cc1377b..6a5dc5a26d 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 a6ebea3943..f5ec956968 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include @@ -75,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(); @@ -134,16 +140,25 @@ void Gaussian::print(const string& name) const { /* ************************************************************************* */ bool Gaussian::equals(const Base& expected, double tol) const { const Gaussian* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; if (typeid(*this) != typeid(*p)) return false; - //if (!sqrt_information_) return true; // ALEX todo; return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } +/* ************************************************************************* */ +Matrix Gaussian::covariance() const { + // Uses a fast version of `covariance = information().inverse();` + const Matrix& R = this->R(); + Matrix I = Matrix::Identity(R.rows(), R.cols()); + // Fast inverse of upper-triangular matrix R using forward-substitution + Matrix Rinv = R.triangularView().solve(I); + // (R' * R)^{-1} = R^{-1} * R^{-1}' + return Rinv * Rinv.transpose(); +} + /* ************************************************************************* */ Vector Gaussian::sigmas() const { - // TODO(frank): can this be done faster? - return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt(); + return Vector(covariance().diagonal()).cwiseSqrt(); } /* ************************************************************************* */ @@ -156,13 +171,6 @@ Vector Gaussian::unwhiten(const Vector& v) const { return backSubstituteUpper(thisR(), v); } -/* ************************************************************************* */ -double Gaussian::Mahalanobis(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; @@ -368,8 +376,19 @@ Vector Constrained::whiten(const Vector& v) const { return c; } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +/* ************************************************************************* */ +double Constrained::error(const Vector& v) const { + Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements + for (size_t i=0; i (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); } @@ -655,7 +674,7 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{ } Robust::shared_ptr Robust::Create( - const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ +const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ return shared_ptr(new Robust(robust,noise)); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6c42fc7ba9..7494e05011 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -90,7 +90,27 @@ namespace gtsam { /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; - virtual double distance(const Vector& v) const = 0; + /// Squared Mahalanobis distance v'*R'*R*v = + 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,26 +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; + 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); + } /** - * Mahalanobis distance v'*R'*R*v = + * error value 0.5 * v'*R'*R*v */ - virtual double Mahalanobis(const Vector& v) const; - - inline virtual double distance(const Vector& v) const { - return Mahalanobis(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 @@ -234,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] @@ -257,7 +281,7 @@ namespace gtsam { virtual Matrix information() const { return R().transpose() * R(); } /// Compute covariance matrix - virtual Matrix covariance() const { return information().inverse(); } + virtual Matrix covariance() const; private: /** Serialization function */ @@ -322,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) @@ -350,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(); } @@ -404,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; @@ -459,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) { @@ -481,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 @@ -501,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 @@ -563,14 +591,14 @@ namespace gtsam { return Variance(dim, 1.0/precision, smart); } - virtual void print(const std::string& name) const; - virtual double Mahalanobis(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 @@ -603,7 +631,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; - virtual ~Unit() {} + ~Unit() {} /** * Create a unit covariance noise model @@ -613,19 +641,19 @@ namespace gtsam { } /// true if a unit noise model, saves slow/clumsy dynamic casting - virtual bool isUnit() const { return true; } - - virtual void print(const std::string& name) const; - virtual double Mahalanobis(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 {} + bool isUnit() const override { return true; } + + 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 */ @@ -674,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_; } @@ -686,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 08307c5abb..a7af7d8d8c 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 f5b278ae51..7752902ba3 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -48,7 +48,12 @@ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { 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/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index d83385d322..1c602a963a 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -183,21 +183,21 @@ void BlockJacobiPreconditioner::clean() { } /***************************************************************************************/ -boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters) { - - if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters) ) { +boost::shared_ptr createPreconditioner( + const boost::shared_ptr params) { + using boost::dynamic_pointer_cast; + if (dynamic_pointer_cast(params)) { return boost::make_shared(); - } - else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast(parameters) ) { + } else if (dynamic_pointer_cast( + params)) { return boost::make_shared(); - } - else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast(parameters) ) { + } else if (auto subgraph = + dynamic_pointer_cast( + params)) { return boost::make_shared(*subgraph); } - throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type"); + throw invalid_argument( + "createPreconditioner: unexpected preconditioner parameter type"); } - -} - - +} // namespace gtsam diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index 2b5cf85fff..0312efe78b 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -102,10 +102,8 @@ class RegularJacobianFactor: public JacobianFactor { DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } - /// Expose base class hessianDiagonal - virtual VectorValues hessianDiagonal() const { - return JacobianFactor::hessianDiagonal(); - } + /// Using the base method + using GaussianFactor::hessianDiagonal; /// Raw memory access version of hessianDiagonal void hessianDiagonal(double* d) const { diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 9f1527bf9f..16c7e73e02 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,6 +11,8 @@ /** * @file Sampler.cpp + * @brief sampling from a diagonal NoiseModel + * @author Frank Dellaert * @author Alex Cunningham */ @@ -18,25 +20,16 @@ namespace gtsam { /* ************************************************************************* */ -Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) - : model_(model), generator_(static_cast(seed)) -{ -} - -/* ************************************************************************* */ -Sampler::Sampler(const Vector& sigmas, int32_t seed) -: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast(seed)) -{ -} +Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, + uint_fast64_t seed) + : model_(model), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(int32_t seed) -: generator_(static_cast(seed)) -{ -} +Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) + : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} /* ************************************************************************* */ -Vector Sampler::sampleDiagonal(const Vector& sigmas) { +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { @@ -55,18 +48,23 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) { } /* ************************************************************************* */ -Vector Sampler::sample() { +Vector Sampler::sample() const { assert(model_.get()); const Vector& sigmas = model_->sigmas(); return sampleDiagonal(sigmas); } /* ************************************************************************* */ -Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} + +Vector Sampler::sampleNewModel( + const noiseModel::Diagonal::shared_ptr& model) const { assert(model.get()); const Vector& sigmas = model->sigmas(); return sampleDiagonal(sigmas); } +#endif /* ************************************************************************* */ -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 93145c31a9..54c240a2b0 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @brief sampling that can be parameterized using a NoiseModel to generate samples from * @file Sampler.h - * the given distribution + * @brief sampling from a NoiseModel + * @author Frank Dellaert * @author Alex Cunningham */ @@ -27,67 +27,74 @@ namespace gtsam { /** * Sampling structure that keeps internal random number generators for * diagonal distributions specified by NoiseModel - * - * This is primarily to allow for variable seeds, and does roughly the same - * thing as sample() in NoiseModel. */ class GTSAM_EXPORT Sampler { -protected: + protected: /** noiseModel created at generation */ noiseModel::Diagonal::shared_ptr model_; /** generator */ - std::mt19937_64 generator_; + mutable std::mt19937_64 generator_; -public: + public: typedef boost::shared_ptr shared_ptr; + /// @name constructors + /// @{ + /** * Create a sampler for the distribution specified by a diagonal NoiseModel * with a manually specified seed * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u); + explicit Sampler(const noiseModel::Diagonal::shared_ptr& model, + uint_fast64_t seed = 42u); /** - * Create a sampler for a distribution specified by a vector of sigmas directly + * Create a sampler for a distribution specified by a vector of sigmas + * directly * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const Vector& sigmas, int32_t seed = 42u); + explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u); - /** - * Create a sampler without a given noisemodel - pass in to sample - * - * NOTE: do not use zero as a seed, it will break the generator - */ - Sampler(int32_t seed = 42u); + /// @} + /// @name access functions + /// @{ + + size_t dim() const { + assert(model_.get()); + return model_->dim(); + } + + Vector sigmas() const { + assert(model_.get()); + return model_->sigmas(); + } - /** access functions */ - size_t dim() const { assert(model_.get()); return model_->dim(); } - Vector sigmas() const { assert(model_.get()); return model_->sigmas(); } const noiseModel::Diagonal::shared_ptr& model() const { return model_; } - /** - * sample from distribution - * NOTE: not const due to need to update the underlying generator - */ - Vector sample(); + /// @} + /// @name basic functionality + /// @{ - /** - * Sample from noisemodel passed in as an argument, - * can be used without having initialized a model for the system. - * - * NOTE: not const due to need to update the underlying generator - */ - Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model); + /// sample from distribution + Vector sample() const; -protected: + /// @} - /** given sigmas for a diagonal model, returns a sample */ - Vector sampleDiagonal(const Vector& sigmas); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + explicit Sampler(uint_fast64_t seed = 42u); + Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const; + /// @} +#endif + protected: + /** given sigmas for a diagonal model, returns a sample */ + Vector sampleDiagonal(const Vector& sigmas) const; }; -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index f18ad9c5f3..0a25617e4a 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,11 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; +#ifdef TBB_GREATER_EQUAL_2020 values_.emplace(key, x.segment(j, n)); +#else + values_.insert(std::make_pair(key, x.segment(j, n))); +#endif j += n; } } @@ -60,7 +64,11 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { +#ifdef TBB_GREATER_EQUAL_2020 values_.emplace(v.key, x.segment(j, v.dimension)); +#else + values_.insert(std::make_pair(v.key, x.segment(j, v.dimension))); +#endif j += v.dimension; } } @@ -70,7 +78,11 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(v.first, Vector::Zero(v.second.size())); +#else + result.values_.insert(std::make_pair(v.first, Vector::Zero(v.second.size()))); +#endif return result; } @@ -84,16 +96,6 @@ namespace gtsam { return result.first; } - /* ************************************************************************* */ - VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { - std::pair result = values_.emplace(j, value); - if(!result.second) - throw std::invalid_argument( - "Requested to emplace variable '" + DefaultKeyFormatter(j) - + "' already in this VectorValues."); - return result.first; - } - /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { @@ -266,7 +268,11 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(j1->first, j1->second + j2->second); +#else + result.values_.insert(std::make_pair(j1->first, j1->second + j2->second)); +#endif return result; } @@ -324,7 +330,11 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(j1->first, j1->second - j2->second); +#else + result.values_.insert(std::make_pair(j1->first, j1->second - j2->second)); +#endif return result; } @@ -340,7 +350,11 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(key_v.first, a * key_v.second); +#else + result.values_.insert(std::make_pair(key_v.first, a * key_v.second)); +#endif return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 46da2d5f9f..9e60ff2aaa 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -179,7 +179,14 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator emplace(Key j, const Vector& value); + template + inline std::pair emplace(Key j, Args&&... args) { +#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020) + return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...)); +#else + return values_.insert(std::make_pair(j, Vector(std::forward(args)...))); +#endif + } /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. @@ -197,8 +204,12 @@ namespace gtsam { * and an iterator to the existing value is returned, along with 'false'. If the value did not * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ - std::pair tryInsert(Key j, const Vector& value) { - return values_.emplace(j, value); + inline std::pair tryInsert(Key j, const Vector& value) { +#ifdef TBB_GREATER_EQUAL_2020 + return values_.emplace(j, value); +#else + return values_.insert(std::make_pair(j, value)); +#endif } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 1afaf79d11..811e071211 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -98,8 +98,13 @@ namespace gtsam // Insert solution into a VectorValues DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - VectorValues::const_iterator r = - collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + if(!result.second) + throw std::runtime_error( + "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal) + + "' that exists."); + + VectorValues::const_iterator r = result.first; myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); } diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 110a1223ac..21146066d7 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -253,7 +253,7 @@ TEST(JacobianFactor, error) /* ************************************************************************* */ TEST(JacobianFactor, matrices_NULL) { - // Make sure everything works with NULL noise model + // Make sure everything works with nullptr noise model JacobianFactor factor(simple::terms, simple::b); Matrix jacobianExpected(3, 9); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 10578627f0..42d68a6037 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -68,10 +68,10 @@ TEST(NoiseModel, constructors) for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); - // test Mahalanobis distance + // test squared Mahalanobis distance double distance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); + DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m) @@ -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/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index bd718500eb..5831d90486 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -10,8 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file testSampler + * @file testSampler.cpp + * @brief unit tests for Sampler class * @author Alex Cunningham + * @author Frank Dellaert */ #include @@ -22,14 +24,15 @@ using namespace gtsam; const double tol = 1e-5; +static const Vector3 kSigmas(1.0, 0.1, 0.0); + /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector3(1.0, 0.1, 0.0); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); + auto model = noiseModel::Diagonal::Sigmas(kSigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); - EXPECT(assert_equal(sigmas, sampler1.sigmas())); - EXPECT(assert_equal(sigmas, sampler2.sigmas())); + EXPECT(assert_equal(kSigmas, sampler1.sigmas())); + EXPECT(assert_equal(kSigmas, sampler2.sigmas())); EXPECT_LONGS_EQUAL(3, sampler1.dim()); EXPECT_LONGS_EQUAL(3, sampler2.dim()); Vector actual1 = sampler1.sample(); @@ -38,5 +41,8 @@ TEST(testSampler, basic) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 0d7e05515a..4604a55dd1 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); + return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 3c6af9332e..7f335152e3 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s, bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) && this->bRef_.equals(e->bRef_, tol); } @@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s, bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) && this->bRef_.equals(e->bRef_, tol); } diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index db588008e9..5a0031caf0 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -139,7 +139,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public At } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits @@ -219,7 +219,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1, } 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 1967e4a569..d7b4b7bf11 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 //------------------------------------------------------------------------------ @@ -174,7 +201,7 @@ void CombinedImuFactor::print(const string& s, //------------------------------------------------------------------------------ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { const This* e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); + return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ @@ -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 ca9b2ca1a5..a895684338 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 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType */ Eigen::Matrix preintMeasCov_; - friend class CombinedImuFactor; public: @@ -136,11 +146,14 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType /// @{ /// 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 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType 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 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType 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 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType 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 @@ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 shared_ptr; #endif + /** Default constructor - only use for serialization */ + CombinedImuFactor() {} + /** * Constructor * @param pose_i Previous pose key @@ -277,12 +309,17 @@ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 - 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/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 05b7259bf0..f2448c4883 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** @@ -73,7 +73,7 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const //*************************************************************************** bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index ff1a530254..d52b4eb298 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -171,7 +171,7 @@ class GTSAM_EXPORT ConstantBias { 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 8e3f8f0a47..7e080ffd55 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 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { /** * 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 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { 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 22897b9d42..a290972e48 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -118,15 +118,13 @@ class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase { 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/NavState.h b/gtsam/navigation/NavState.h index e9afcd3ac8..f66e9d7a93 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -64,7 +64,7 @@ class GTSAM_EXPORT NavState { R_(pose.rotation()), t_(pose.translation()), v_(v) { } /// Construct from SO(3) and R^6 - NavState(const Matrix3& R, const Vector9 tv) : + NavState(const Matrix3& R, const Vector6& tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } /// Named constructor with derivatives diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 8c29d85ddb..c5d48b734b 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 71ef5d08fb..0e0559a32d 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.cpp b/gtsam/navigation/PreintegrationBase.cpp index 0e8cf67c98..4af752ac0c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -115,20 +115,21 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { - // TODO(frank): make sure this stuff is still correct Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, - H2 ? &D_biasCorrected_bias : 0); + H2 ? &D_biasCorrected_bias : nullptr); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, - H2 ? &D_delta_biasCorrected : 0); + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, + H2 ? &D_delta_biasCorrected : nullptr); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; - NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); + NavState state_j = state_i.retract(xi, + H1 ? &D_predict_state : nullptr, + H2 || H2 ? &D_predict_delta : nullptr); if (H1) *H1 = D_predict_state + D_predict_delta * D_delta_state; if (H2) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9926d207a4..29d7814b52 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 61cd1617c2..2298bb6967 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 0fb54a3588..9ae66e6785 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -29,10 +29,19 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration Vector3 n_gravity; ///< Gravity vector in nav frame + /// Default constructor for serialization only + PreintegrationParams() + : PreintegratedRotationParams(), + accelerometerCovariance(I_3x3), + integrationCovariance(I_3x3), + use2ndOrderCoriolis(false), + n_gravity(0, 0, -1) {} + /// 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) {} @@ -47,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; } @@ -60,18 +69,15 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } protected: - /// Default constructor for serialization only: uninitialized! - PreintegrationParams() {} /** Serialization function */ friend class boost::serialization::access; 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); } @@ -79,7 +85,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { #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/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 5fb9f78d77..649d0fe12f 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner { const Bias estimatedBias_; // Create two samplers for acceleration and omega noise - mutable Sampler gyroSampler_, accSampler_; + Sampler gyroSampler_, accSampler_; public: ScenarioRunner(const Scenario& scenario, const SharedParams& p, diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 55efdb1510..56d7aa6d30 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -68,7 +68,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, Matrix3 w_tangent_H_theta, invH; const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const Rot3 R(local.expmap()); + const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; @@ -110,7 +110,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc, Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - // Possibly correct for sensor pose + // Possibly correct for sensor pose by converting to body frame Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; if (p().body_P_sensor) boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 11945e53af..1b51b4e1e3 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -132,16 +132,14 @@ class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase { 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/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index d32553b94a..74acf69da2 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -15,6 +15,7 @@ * @author Krunal Chande * @author Luca Carlone * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) { // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H3e, H3a, 1e-5)); - // FIXME!! DOes not work. Different matrix dimensions. + // 1e-5 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ @@ -341,7 +342,7 @@ TEST( AHRSFactor, fistOrderExponential ) { //****************************************************************************** TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point - Vector3 bias; ///< Current estimate of rotation rate bias + Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 460695ec69..474b00add2 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -717,7 +717,6 @@ TEST(ImuFactor, bodyPSensorNoBias) { /* ************************************************************************* */ #include #include -#include #include #include @@ -763,20 +762,17 @@ TEST(ImuFactor, bodyPSensorWithBias) { NonlinearFactorGraph graph; Values values; - PriorFactor priorPose(X(0), Pose3(), priorNoisePose); - graph.add(priorPose); + graph.addPrior(X(0), Pose3(), priorNoisePose); values.insert(X(0), Pose3()); - PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); - graph.add(priorVel); + graph.addPrior(V(0), zeroVel, priorNoiseVel); values.insert(V(0), zeroVel); // The key to this test is that we specify the bias, in the sensor frame, as known a priori // We also create factors below that encode our assumption that this bias is constant over time // In theory, after optimization, we should recover that same bias estimate Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) - PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); - graph.add(priorBiasFactor); + graph.addPrior(B(0), priorBias, priorNoiseBias); values.insert(B(0), priorBias); // Now add IMU factors and bias noise models diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index 9f9781d2cc..ed72e18e9b 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)); + + P pim(p, priorBias); - // measurements are needed for non-inf noise model, otherwise will throw err + // 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,11 +66,45 @@ 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)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + 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)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp similarity index 100% rename from gtsam/navigation/tests/testScenarios.cpp rename to gtsam/navigation/tests/testScenarioRunner.cpp diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index ff059ef780..682cca29aa 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/Expression.h b/gtsam/nonlinear/Expression.h index 7d02d479c3..9e8aa3f291 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -66,7 +66,7 @@ class Expression { // Expressions wrap trees of functions that can evaluate their own derivatives. // The meta-functions below are useful to specify the type of those functions. // Example, a function taking a camera and a 3D point and yielding a 2D point: - // Expression::BinaryFunction::type + // Expression::BinaryFunction,Point3>::type template struct UnaryFunction { typedef boost::function< diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04d82fe9a0..c42b2bdfcf 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -209,7 +209,7 @@ class ExpressionFactor: public NoiseModelFactor { // 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 0000000000..a831989674 --- /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/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index 5cf962e43d..c6e1001c44 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params { /// When you will be removing many factors, e.g. when using ISAM2 as a /// fixed-lag smoother, enable this option to add factors in the first - /// available factor slots, to avoid accumulating NULL factor slots, at the + /// available factor slots, to avoid accumulating nullptr factor slots, at the /// cost of having to search for slots every time a factor is added. bool findUnusedFactorSlots; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 4d928482e6..1bba570516 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -175,6 +175,8 @@ class NonlinearEquality: public NoiseModelFactor1 { /// @} + GTSAM_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ @@ -263,6 +265,8 @@ class NonlinearEquality1: public NoiseModelFactor1 { traits::Print(value_, "Value"); } + GTSAM_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ @@ -327,6 +331,8 @@ class NonlinearEquality2: public NoiseModelFactor2 { return traits::Local(x1,x2); } + GTSAM_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index ee14e8073a..1cfcba274b 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/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index d4b9fbb68e..5f6cdcc985 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -54,26 +54,33 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; ss << "Factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter); cout << endl; } } /* ************************************************************************* */ void NonlinearFactorGraph::printErrors(const Values& values, const std::string& str, - const KeyFormatter& keyFormatter) const { + const KeyFormatter& keyFormatter, + const std::function& printCondition) const +{ cout << str << "size: " << size() << endl << endl; for (size_t i = 0; i < factors_.size(); i++) { + const sharedFactor& factor = factors_[i]; + const double errorValue = (factor != nullptr ? factors_[i]->error(values) : .0); + if (!printCondition(factor.get(),errorValue,i)) + continue; // User-provided filter did not pass + stringstream ss; ss << "Factor " << i << ": "; - if (factors_[i] == NULL) { - cout << "NULL" << endl; + if (factor == nullptr) { + cout << "nullptr" << "\n"; } else { - factors_[i]->print(ss.str(), keyFormatter); - cout << "error = " << factors_[i]->error(values) << endl; + factor->print(ss.str(), keyFormatter); + cout << "error = " << errorValue << "\n"; } - cout << endl; + cout << endl; // only one "endl" at end might be faster, \n for each factor } } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 902a47a0f3..0e17700d04 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -103,7 +104,9 @@ namespace gtsam { /** print errors along with factors*/ void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = [](const Factor *,double, size_t) {return true;}) const; /** Test equality */ bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; @@ -202,6 +205,33 @@ namespace gtsam { push_back(boost::make_shared >(R, z, h)); } + /** + * Convenience method which adds a PriorFactor to the factor graph. + * @param key Variable key + * @param prior The variable's prior value + * @param model Noise model for prior factor + */ + template + void addPrior(Key key, const T& prior, + const SharedNoiseModel& model = nullptr) { + emplace_shared>(key, prior, model); + } + + /** + * Convenience method which adds a PriorFactor to the factor graph. + * @param key Variable key + * @param prior The variable's prior value + * @param covariance Covariance matrix. + * + * Note that the smart noise model associated with the prior factor + * automatically picks the right noise model (e.g. a diagonal noise model + * if the provided covariance matrix is diagonal). + */ + template + void addPrior(Key key, const T& prior, const Matrix& covariance) { + emplace_shared>(key, prior, covariance); + } + private: /** diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0c5d99c0f6..328a3facf4 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -147,13 +147,15 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) - throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + throw std::runtime_error( + "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); - if (boost::shared_ptr pcg = - boost::dynamic_pointer_cast(params.iterativeParams)) { + if (auto pcg = boost::dynamic_pointer_cast( + params.iterativeParams)) { delta = PCGSolver(*pcg).optimize(gfg); - } else if (boost::shared_ptr spcg = - boost::dynamic_pointer_cast(params.iterativeParams)) { + } else if (auto spcg = + boost::dynamic_pointer_cast( + params.iterativeParams)) { if (!params.ordering) throw std::runtime_error("SubgraphSolver needs an ordering"); delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h new file mode 100644 index 0000000000..0afbaa5883 --- /dev/null +++ b/gtsam/nonlinear/PriorFactor.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * 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 PriorFactor.h + * @author Frank Dellaert + **/ +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /** + * A class for a soft prior on any Value type + * @addtogroup SLAM + */ + template + class PriorFactor: public NoiseModelFactor1 { + + public: + typedef VALUE T; + + private: + + typedef NoiseModelFactor1 Base; + + VALUE prior_; /** The measurement */ + + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + /// shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr > shared_ptr; + + /// Typedef to this class + typedef PriorFactor This; + + /** default constructor - only use for serialization */ + PriorFactor() {} + + virtual ~PriorFactor() {} + + /** Constructor */ + PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : + Base(model, key), prior_(prior) { + } + + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; + traits::Print(prior_, " prior mean: "); + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; + } + + /** 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(prior_, e->prior_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const T& x, boost::optional H = boost::none) const { + if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); + // manifold equivalent of z-x -> Local(x,z) + // TODO(ASL) Add Jacobians. + return -traits::Local(x, prior_); + } + + const VALUE & prior() const { return prior_; } + + 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(prior_); + } + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + }; + +} /// namespace gtsam diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 1bd8b3a737..98790ccd97 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -232,10 +232,21 @@ namespace gtsam { /* ************************************************************************* */ const char* ValuesIncorrectType::what() const throw() { - if(message_.empty()) - message_ = - "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " + - std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name()); + if(message_.empty()) { + std::string storedTypeName = demangle(storedTypeId_.name()); + std::string requestedTypeName = demangle(requestedTypeId_.name()); + + if (storedTypeName == requestedTypeName) { + message_ = "WARNING: Detected types with same name but different `typeid`. \ + This is usually caused by incorrect linking/inlining settings when compiling libraries using GTSAM. \ + If you are a user, please report to the author of the library using GTSAM. \ + If you are a package maintainer, please consult `cmake/GtsamPybindWrap.cmake`, line 74 for details."; + } else { + message_ = + "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " + + storedTypeName + " but requested type was " + requestedTypeName; + } + } return message_.c_str(); } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e48b11b32b..041aa3441e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -276,8 +276,8 @@ namespace gtsam { void update(Key j, const Value& val); /** Templated version to update a variable with the given j, - * throws KeyAlreadyExists if j is already present - * if no chart is specified, the DefaultChart is used + * throws KeyDoesNotExist if j is not present. + * If no chart is specified, the DefaultChart is used. */ template void update(Key j, const T& val); diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ace0aaea8f..2943b5e682 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 6752d7fc05..0ae37f1307 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -84,7 +84,7 @@ class ExpressionNode { /// Streaming GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ExpressionNode& node) { - os << "Expression of type " << typeid(T).name(); + os << "Expression of type " << demangle(typeid(T).name()); if (node.traceSize_ > 0) os << ", trace size = " << node.traceSize_; os << "\n"; return os; @@ -150,7 +150,7 @@ class ConstantExpression: public ExpressionNode { return constant_; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; //----------------------------------------------------------------------------- @@ -219,7 +219,7 @@ static void PrintJacobianAndTrace(const std::string& indent, const typename Jacobian::type& dTdA, const ExecutionTrace trace) { static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(A).name() + std::cout << indent << "D(" << demangle(typeid(T).name()) << ")/D(" << demangle(typeid(A).name()) << ") = " << dTdA.format(kMatlabFormat) << std::endl; trace.print(indent); } @@ -605,7 +605,7 @@ class ScalarMultiplyNode : public ExpressionNode { /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "ScalarMultiplyNode::Record {" << std::endl; - std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(T).name() + std::cout << indent << "D(" << demangle(typeid(T).name()) << ")/D(" << demangle(typeid(T).name()) << ") = " << scalar_dTdA << std::endl; trace.print(); std::cout << indent << "}" << std::endl; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index a85118c005..2f4f21286b 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); Vector3 X = point; +#ifdef GTSAM_POSE3_EXPMAP +Vector2 expectedMeasurement(1.3124675, 1.2057287); +#else Vector2 expectedMeasurement(1.2431567, 1.2525694); +#endif Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); } @@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22(adapted, P, X); // Check that Local worked as expected TEST(AdaptAutoDiff, Local) { using namespace example; +#ifdef GTSAM_POSE3_EXPMAP + Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished(); +#else Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished(); +#endif EXPECT(equal_with_abs_tol(expectedP, P)); Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely! EXPECT(equal_with_abs_tol(expectedX, X)); diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 494a59fff8..c5ccc0f525 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -internal::JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(nullptr); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index c2b245780f..58f76089a1 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 0000000000..12dd6b91c0 --- /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/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index b8eee540d4..2f624f527d 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -589,6 +589,23 @@ TEST(Values, MatrixDynamicInsertFixedRead) { CHECK_EXCEPTION(values.at(key1), exception); } +TEST(Values, Demangle) { + Values values; + Matrix13 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix) [\n 5, 6, 7\n]\n\n"; + + stringstream buffer; + streambuf * old = cout.rdbuf(buffer.rdbuf()); + + values.print(); + + string actual = buffer.str(); + cout.rdbuf(old); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 867db70e05..a78f7a2d05 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -169,7 +169,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { } /// Insert a number of initial point values by backprojecting -void insertBackprojections(Values& values, const SimpleCamera& camera, +void insertBackprojections(Values& values, const PinholeCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); @@ -237,13 +237,16 @@ Values localToWorld(const Values& local, const Pose2& base, // if value is a Pose2, compose it with base pose Pose2 pose = local.at(key); world.insert(key, base.compose(pose)); - } catch (std::exception e1) { + } catch (const std::exception& e1) { try { // if value is a Point2, transform it from base pose Point2 point = local.at(key); world.insert(key, base.transformFrom(point)); - } catch (std::exception e2) { + } catch (const std::exception& e2) { // if not Pose2 or Point2, do nothing + #ifndef NDEBUG + std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl; + #endif } } } diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5d57886f52..54d4a43c07 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -19,7 +19,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -353,11 +354,13 @@ TEST(RangeFactor, Point3) { } /* ************************************************************************* */ -// Do tests with SimpleCamera +// Do tests with PinholeCamera TEST( RangeFactor, Camera) { - RangeFactor factor1(poseKey, pointKey, measurement, model); - RangeFactor factor2(poseKey, pointKey, measurement, model); - RangeFactor factor3(poseKey, pointKey, measurement, model); + using Camera = PinholeCamera; + + RangeFactor factor1(poseKey, pointKey, measurement, model); + RangeFactor factor2(poseKey, pointKey, measurement, model); + RangeFactor factor3(poseKey, pointKey, measurement, model); } /* ************************************************************************* */ diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h new file mode 100644 index 0000000000..d63633d7e8 --- /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 0000000000..aeeae688fb --- /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 0000000000..bb3c3cdb18 --- /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 0000000000..37e8b6c0f9 --- /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/AntiFactor.h b/gtsam/slam/AntiFactor.h index b52e115fd3..9e4f7db5a1 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -67,7 +67,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); + return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index f949514e39..b1d4904aa4 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -81,7 +81,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -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.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp index e27bbc8c5a..5397c2e57d 100644 --- a/gtsam/slam/EssentialMatrixConstraint.cpp +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s, bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && this->measuredE_.equals(e->measuredE_, tol); } diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 179200fe1a..e474ce5b36 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -105,7 +105,7 @@ class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 #include -#include +#include #include namespace gtsam { @@ -81,7 +81,7 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -201,7 +201,7 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -286,7 +286,7 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { } 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 0000000000..904addb030 --- /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 0000000000..a73445ae0c --- /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/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 1e398cd993..3d4a4d40df 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index b94714dd65..bc906d24e6 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -65,7 +65,7 @@ class JacobianFactorSVD: public RegularJacobianFactor { Base() { size_t numKeys = Enull.rows() / ZDim; size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? - // PLAIN NULL SPACE TRICK + // PLAIN nullptr SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // for(const KeyMatrixZD& it: Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index 076b0ff0c0..f10cc7e422 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -20,7 +20,6 @@ #include #include #include -#include using namespace std; @@ -33,7 +32,7 @@ T FindKarcherMeanImpl(const vector& rotations) { NonlinearFactorGraph graph; static const Key kKey(0); for (const auto& R : rotations) { - graph.emplace_shared >(kKey, R); + graph.addPrior(kKey, R); } Values initial; initial.insert(kKey, T()); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 5b0c6a6b93..56968301a8 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s, bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); } diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 78030ff345..f4ce1520a2 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -62,7 +62,7 @@ class PoseRotationPrior : public NoiseModelFactor1 { /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); } /** print contents */ diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index c38d13b582..516582d830 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -76,7 +76,7 @@ class PoseTranslationPrior : public NoiseModelFactor1 { /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 5b085652f1..335eb2b5ac 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -15,106 +15,6 @@ **/ #pragma once -#include -#include - -#include - -namespace gtsam { - - /** - * A class for a soft prior on any Value type - * @addtogroup SLAM - */ - template - class PriorFactor: public NoiseModelFactor1 { - - public: - typedef VALUE T; - - private: - - typedef NoiseModelFactor1 Base; - - VALUE prior_; /** The measurement */ - - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T) - - public: - - /// shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr > shared_ptr; - - /// Typedef to this class - typedef PriorFactor This; - - /** default constructor - only use for serialization */ - PriorFactor() {} - - virtual ~PriorFactor() {} - - /** Constructor */ - PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : - Base(model, key), prior_(prior) { - } - - /** Convenience constructor that takes a full covariance argument */ - PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : - Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - traits::Print(prior_, " prior mean: "); - if (this->noiseModel_) - this->noiseModel_->print(" noise model: "); - else - std::cout << "no noise model" << std::endl; - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); - } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const T& x, boost::optional H = boost::none) const { - if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); - // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. - return -traits::Local(x, prior_); - } - - const VALUE & prior() const { return prior_; } - - 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(prior_); - } - - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) - }; - -} /// namespace gtsam +// Note: PriorFactor has been moved to gtsam/nonlinear/PriorFactor.h. This file +// has been left here for backwards compatibility. +#include diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 4a8a6b138a..0bed15fdcd 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -21,7 +21,8 @@ #pragma once #include -#include +#include +#include #include namespace gtsam { @@ -188,7 +189,7 @@ namespace gtsam { } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 07b7498117..71474a8ab2 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -10,7 +10,11 @@ #include #include #include + #include +#include +#include +#include namespace gtsam { @@ -76,7 +80,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { /// print void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + DefaultKeyFormatter) const override { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); for (size_t pos = 0; pos < size(); ++pos) { @@ -88,7 +92,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { } /// equals - bool equals(const GaussianFactor& lf, double tol) const { + bool equals(const GaussianFactor& lf, double tol) const override { const This* f = dynamic_cast(&lf); if (!f) return false; @@ -104,48 +108,48 @@ class RegularImplicitSchurFactor: public GaussianFactor { } /// Degrees of freedom of camera - virtual DenseIndex getDim(const_iterator variable) const { + DenseIndex getDim(const_iterator variable) const override { return D; } - virtual void updateHessian(const KeyVector& keys, - SymmetricBlockMatrix* info) const { + void updateHessian(const KeyVector& keys, + SymmetricBlockMatrix* info) const override { throw std::runtime_error( "RegularImplicitSchurFactor::updateHessian non implemented"); } - virtual Matrix augmentedJacobian() const { + Matrix augmentedJacobian() const override { throw std::runtime_error( "RegularImplicitSchurFactor::augmentedJacobian non implemented"); return Matrix(); } - virtual std::pair jacobian() const { + std::pair jacobian() const override { throw std::runtime_error( "RegularImplicitSchurFactor::jacobian non implemented"); return std::make_pair(Matrix(), Vector()); } /// *Compute* full augmented information matrix - virtual Matrix augmentedInformation() const { - + Matrix augmentedInformation() const override { // Do the Schur complement - SymmetricBlockMatrix augmentedHessian = // + SymmetricBlockMatrix augmentedHessian = Set::SchurComplement(FBlocks_, E_, b_); return augmentedHessian.selfadjointView(); } /// *Compute* full information matrix - virtual Matrix information() const { + Matrix information() const override { Matrix augmented = augmentedInformation(); int m = this->keys_.size(); size_t M = D * m; return augmented.block(0, 0, M, M); } - /// Return the diagonal of the Hessian for this factor - virtual VectorValues hessianDiagonal() const { - // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); - VectorValues d; + /// Using the base method + using GaussianFactor::hessianDiagonal; + /// Add the diagonal of the Hessian for this factor to existing VectorValues + void hessianDiagonalAdd(VectorValues &d) const override { + // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); for (size_t k = 0; k < size(); ++k) { // for each camera Key j = keys_[k]; @@ -153,7 +157,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { // D x 3 = (D x ZDim) * (ZDim x 3) const MatrixZD& Fj = FBlocks_[k]; Eigen::Matrix FtE = Fj.transpose() - * E_.block(ZDim * k, 0); + * E_.block(ZDim * k, 0); Eigen::Matrix dj; for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian @@ -163,16 +167,19 @@ class RegularImplicitSchurFactor: public GaussianFactor { // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); } - d.insert(j, dj); + + auto result = d.emplace(j, dj); + if(!result.second) { + result.first->second += dj; + } } - return d; } /** * @brief add the contribution of this factor to the diagonal of the hessian * d(output) = d(input) + deltaHessianFactor */ - virtual void hessianDiagonal(double* d) const { + void hessianDiagonal(double* d) const override { // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -198,7 +205,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { } /// Return the block diagonal of the Hessian for this factor - virtual std::map hessianBlockDiagonal() const { + std::map hessianBlockDiagonal() const override { std::map blocks; // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { @@ -223,17 +230,18 @@ class RegularImplicitSchurFactor: public GaussianFactor { return blocks; } - virtual GaussianFactor::shared_ptr clone() const { + GaussianFactor::shared_ptr clone() const override { return boost::make_shared >(keys_, FBlocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::clone non implemented"); } - virtual bool empty() const { + + bool empty() const override { return false; } - virtual GaussianFactor::shared_ptr negate() const { + GaussianFactor::shared_ptr negate() const override { return boost::make_shared >(keys_, FBlocks_, PointCovariance_, E_, b_); throw std::runtime_error( @@ -284,7 +292,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { * f = nonlinear error * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f */ - virtual double error(const VectorValues& x) const { + double error(const VectorValues& x) const override { // resize does not do malloc if correct size e1.resize(size()); @@ -379,13 +387,12 @@ class RegularImplicitSchurFactor: public GaussianFactor { void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const { } - ; /** * @brief Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { + VectorValues& y) const override { // resize does not do malloc if correct size e1.resize(size()); @@ -428,7 +435,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { /** * Calculate gradient, which is -F'Q*b, see paper */ - VectorValues gradientAtZero() const { + VectorValues gradientAtZero() const override { // calculate Q*b e1.resize(size()); e2.resize(size()); @@ -450,7 +457,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { /** * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS */ - virtual void gradientAtZero(double* d) const { + void gradientAtZero(double* d) const override { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -470,7 +477,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { } /// Gradient wrt a key at any values - Vector gradient(Key key, const VectorValues& x) const { + Vector gradient(Key key, const VectorValues& x) const override { throw std::runtime_error( "gradient for RegularImplicitSchurFactor is not implemented yet"); } diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index b6ccc36a2e..948fffe134 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -113,7 +113,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { 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 34f9b9e9f1..1c80b87446 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -81,7 +81,7 @@ class SmartFactorBase: public NonlinearFactor { 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 @@ class SmartFactorBase: public NonlinearFactor { 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 45d009b1cb..6699359949 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -252,7 +253,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, is.seekg(0, ios::beg); // If asked, create a sampler with random number generator - Sampler sampler; + std::unique_ptr sampler; if (addNoise) { noiseModel::Diagonal::shared_ptr noise; if (model) @@ -261,7 +262,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, throw invalid_argument( "gtsam::load2D: invalid noise model for adding noise" "(current version assumes diagonal noise model)!"); - sampler = Sampler(noise); + sampler.reset(new Sampler(noise)); } // Parse the pose constraints @@ -289,7 +290,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, model = modelInFile; if (addNoise) - l1Xl2 = l1Xl2.retract(sampler.sample()); + l1Xl2 = l1Xl2.retract(sampler->sample()); // Insert vertices if pure odometry file if (!initial->exists(id1)) @@ -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 3ab199bab4..0327994297 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/expressions.h b/gtsam/slam/expressions.h index 4294d17d11..d60923d8e5 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -11,6 +11,7 @@ #include #include #include +#include #include namespace gtsam { @@ -31,6 +32,7 @@ typedef Expression Point3_; typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; +typedef Expression Line3_; inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformTo, p); @@ -40,6 +42,12 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformFrom, p); } +inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) { + Line3 (*f)(const Pose3 &, const Line3 &, + OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo; + return Line3_(f, wTc, wL); +} + namespace internal { // define getter that returns value rather than reference inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 34c8385e8e..76edc8b9dd 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index e34e132797..4188f56391 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -90,7 +89,7 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.emplace_shared >(1, pose1, sigma); + graph.addPrior(1, pose1, sigma); graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 9a3c797b20..8088ab18ac 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) -{ +TEST(dataSet, readG2oCheckDeterminants) { + const string g2oFile = findExampleDataFile("toyExample.g2o"); + + // 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); + } + + // 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); + } +} + +/* ************************************************************************* */ +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, KernelFunctionTypeHUBER); + 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::Huber::Create(1.345), 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) -{ +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); + boost::tie(actualGraph, actualValues) = + readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); - 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::Tukey::Create(4.6851), 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)); } /* ************************************************************************* */ @@ -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/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6d6daa33da..95db611d76 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp new file mode 100644 index 0000000000..9cb0c19fa9 --- /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/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 8e0b5ad8f6..e19a41b8df 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -388,8 +387,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { graph.emplace_shared< RangeFactor >(X(0), X(1), 2., noiseModel::Isotropic::Sigma(1, 1.)); - graph.emplace_shared< - PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), noiseModel::Isotropic::Sigma(6, 1.)); Values init; @@ -413,14 +411,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.emplace_shared< - PriorFactor >(X(0), CalibratedCamera(), + graph.addPrior(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.)); graph.emplace_shared< RangeFactor >(X(0), X(1), 2., noiseModel::Isotropic::Sigma(1, 1.)); - graph.emplace_shared< - PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), noiseModel::Isotropic::Sigma(6, 1.)); Values init; diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 6fe8b3d7c1..ba41cdc9b4 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -67,7 +66,7 @@ NonlinearFactorGraph graph() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); - g.add(PriorFactor(x0, pose0, model)); + g.addPrior(x0, pose0, model); return g; } @@ -78,7 +77,7 @@ NonlinearFactorGraph graph2() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin - g.add(PriorFactor(x0, pose0, model)); + g.addPrior(x0, pose0, model); return g; } } @@ -267,7 +266,7 @@ TEST( InitializePose3, initializePoses ) bool is3D = true; boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D); noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - inputGraph->add(PriorFactor(0, Pose3(), priorModel)); + inputGraph->addPrior(0, Pose3(), priorModel); Values initial = InitializePose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index f8157c1160..55449d86ed 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -61,7 +60,7 @@ NonlinearFactorGraph graph() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); - g.add(PriorFactor(x0, pose0, model)); + g.addPrior(x0, pose0, model); return g; } } @@ -167,7 +166,7 @@ TEST( Lago, smallGraphVectorValuesSP ) { TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1, model)); + g.addPrior(x1, simpleLago::pose1, model); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -180,7 +179,7 @@ TEST( Lago, multiplePosePriors ) { /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1, model)); + g.addPrior(x1, simpleLago::pose1, model); VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -194,7 +193,7 @@ TEST( Lago, multiplePosePriorsSP ) { TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1.theta(), model)); + g.addPrior(x1, simpleLago::pose1.theta(), model); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -207,7 +206,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1.theta(), model)); + g.addPrior(x1, simpleLago::pose1.theta(), model); VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -267,7 +266,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.addPrior(0, Pose2(), priorModel); VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; @@ -302,7 +301,7 @@ TEST( Lago, largeGraphNoisy ) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.addPrior(0, Pose2(), priorModel); Values actual = lago::initialize(graphWithPrior); diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index ea42a1ecdd..81f67f1ee2 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -49,10 +48,9 @@ TEST (OrientedPlane3Factor, lm_translation_error) { Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - PriorFactor pose_prior(init_sym, init_pose, - noiseModel::Diagonal::Sigmas(sigmas)); + new_graph.addPrior( + init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas)); new_values.insert(init_sym, init_pose); - new_graph.add(pose_prior); // Add two landmark measurements, differing in range new_values.insert(lm_sym, test_lm0); @@ -94,11 +92,10 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - PriorFactor pose_prior(init_sym, init_pose, + new_graph.addPrior(init_sym, init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); new_values.insert(init_sym, init_pose); - new_graph.add(pose_prior); // // Add two landmark measurements, differing in angle new_values.insert(lm_sym, test_lm0); @@ -175,7 +172,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b26d633f5b..2dc083cb23 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -6,7 +6,7 @@ */ #include -#include +#include #include using namespace std; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index b85dd891aa..09ee61c97b 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -41,7 +41,7 @@ using namespace gtsam; const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones(); -const vector > FBlocks = list_of(F0)(F1)(F3); +const vector > FBlocks {F0, F1, F3}; const KeyVector keys {0, 1, 3}; // RHS and sigmas const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index f69f4c1135..fd771f1025 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/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 7b3158e9ac..1fd06cc9f4 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -195,8 +195,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimize ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior(c1, cam1, noisePrior); + graph.addPrior(c2, cam2, noisePrior); // Create initial estimate Values initial; @@ -304,8 +304,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior(c1, cam1, noisePrior); + graph.addPrior(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -378,8 +378,8 @@ TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) { graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior(c1, cam1, noisePrior); + graph.addPrior(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -453,8 +453,8 @@ TEST(SmartProjectionFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior(c1, cam1, noisePrior); + graph.addPrior(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -526,8 +526,8 @@ TEST(SmartProjectionFactor, Cal3Bundler2 ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior(c1, cam1, noisePrior); + graph.addPrior(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f833941bcc..0cc5e8f554 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -159,7 +159,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { factor.computeJacobians(Fs, E, b, cameras, *point); double actualError3 = b.squaredNorm(); EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); } /* *************************************************************************/ @@ -243,8 +243,8 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, wTb1, noisePrior); - graph.emplace_shared >(x2, wTb2, noisePrior); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -302,8 +302,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); Values groundTruth; groundTruth.insert(x1, cam1.pose()); @@ -329,7 +329,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } /* *************************************************************************/ @@ -407,10 +407,10 @@ TEST( SmartProjectionPoseFactor, Factors ) { boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); - EXPECT(assert_equal(expected, *actual, 1e-8)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } { @@ -448,15 +448,15 @@ TEST( SmartProjectionPoseFactor, Factors ) { SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); Matrix3 P = (E.transpose() * E).inverse(); JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); - EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); boost::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); CHECK(actualQ); - EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); EXPECT(assert_equal(expectedQ, *actualQ)); - EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-8); - EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); // Whiten for RegularImplicitSchurFactor (does not have noise model) model->WhitenSystem(E, b); @@ -470,11 +470,11 @@ TEST( SmartProjectionPoseFactor, Factors ) { boost::shared_ptr > actual = smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } { @@ -484,15 +484,15 @@ TEST( SmartProjectionPoseFactor, Factors ) { double s = sigma * sin(M_PI_4); SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); boost::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); CHECK(actual); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } } @@ -525,8 +525,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -589,8 +589,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -603,7 +603,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } /* *************************************************************************/ @@ -647,8 +647,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -716,8 +716,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); Values values; values.insert(x1, cam1.pose()); @@ -766,8 +766,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -779,7 +779,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } /* *************************************************************************/ @@ -806,8 +806,8 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.emplace_shared >(x1, level_pose, noisePrior); - graph.emplace_shared >(x2, pose_right, noisePrior); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -899,7 +899,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian - EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); Matrix AugInformationMatrix = factor1->augmentedInformation() + factor2->augmentedInformation() + factor3->augmentedInformation(); @@ -908,7 +908,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian - EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); } /* *************************************************************************/ @@ -948,7 +948,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); @@ -1012,7 +1012,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); @@ -1223,8 +1223,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1296,7 +1296,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 1d2baefee7..4bbef6530f 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ static const boost::shared_ptr sharedCal = // // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -SimpleCamera camera1(pose1, *sharedCal); +PinholeCamera camera1(pose1, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); @@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) { Key pointKey(1); SharedNoiseModel model; - typedef TriangulationFactor Factor; + typedef TriangulationFactor > Factor; Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 8a9a136480..01ac00b37f 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/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 53ba83fade..010b327102 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -48,7 +48,7 @@ endforeach(subdir) set(library_name GTSAM_UNSTABLE) # For substitution in dllexport.h.in configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_unstable_srcs "${PROJECT_BINARY_DIR}/dllexport.h") -install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam_unstable) +install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable) # assemble gtsam_unstable components set(gtsam_unstable_srcs @@ -101,9 +101,9 @@ endif() install( TARGETS gtsam_unstable EXPORT GTSAM-exports - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin) + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index fd675d0d5d..96424324bf 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -72,7 +72,7 @@ namespace gtsam { }; // Node // We store a shared pointer to the root of the functional tree - // composed of Node classes. If root_==NULL, the tree is empty. + // composed of Node classes. If root_==nullptr, the tree is empty. typedef boost::shared_ptr sharedNode; sharedNode root_; @@ -223,7 +223,7 @@ namespace gtsam { /** Return height of the tree, 0 if empty */ size_t height() const { - return (root_ != NULL) ? root_->height_ : 0; + return (root_ != nullptr) ? root_->height_ : 0; } /** return size of the tree */ diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 494f2731f2..c7e46e4003 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 41e75432ff..95629fb43b 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include @@ -61,7 +60,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('K', 0), *noisy_K); noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); - graph.emplace_shared >(Symbol('K', 0), *noisy_K, calNoise); + graph.addPrior(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -77,7 +76,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); + graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys size_t x, l; diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 429a2c2b2e..939d3a5c84 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -34,7 +34,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -86,7 +85,7 @@ int main(int argc, char** argv) { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; - newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); + newFactors.addPrior(priorKey, priorMean, priorNoise); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds; diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index dc9b005803..16ede58e07 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -30,7 +30,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -80,7 +79,7 @@ int main(int argc, char** argv) { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; - newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); + newFactors.addPrior(priorKey, priorMean, priorNoise); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 9dbeeac89b..5fdc7a7432 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -37,7 +37,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. -#include #include #include #include @@ -127,7 +126,7 @@ int main(int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(0, pose0, priorNoise)); + newFactors.addPrior(0, pose0, priorNoise); ofstream os2("rangeResultLM.txt"); ofstream os3("rangeResultSR.txt"); diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index a63a0ba20a..90b2a30ff3 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -37,7 +37,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. -#include #include #include @@ -124,7 +123,7 @@ int main(int argc, char** argv) { // Add prior on first pose Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(0, pose0, priorNoise)); + newFactors.addPrior(0, pose0, priorNoise); // initialize points (Gaussian) Values initial; diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp new file mode 100644 index 0000000000..f72b2cf952 --- /dev/null +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * 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 TimeOfArrivalExample.cpp + * @brief Track a moving object "Time of Arrival" measurements at 4 + * microphones. + * @author Frank Dellaert + * @author Jay Chakravarty + * @date March 2020 + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// units +static const double ms = 1e-3; +static const double cm = 1e-2; + +// Instantiate functor with speed of sound value +static const TimeOfArrival kTimeOfArrival(330); + +/* ************************************************************************* */ +// Create microphones +vector defineMicrophones() { + const double height = 0.5; + vector microphones; + microphones.push_back(Point3(0, 0, height)); + microphones.push_back(Point3(403 * cm, 0, height)); + microphones.push_back(Point3(403 * cm, 403 * cm, height)); + microphones.push_back(Point3(0, 403 * cm, 2 * height)); + return microphones; +} + +/* ************************************************************************* */ +// Create ground truth trajectory +vector createTrajectory(size_t n) { + vector trajectory; + double timeOfEvent = 10; + // simulate emitting a sound every second while moving on straight line + for (size_t key = 0; key < n; key++) { + trajectory.push_back( + Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm)); + timeOfEvent += 1; + } + return trajectory; +} + +/* ************************************************************************* */ +// Simulate time-of-arrival measurements for a single event +vector simulateTOA(const vector& microphones, + const Event& event) { + size_t K = microphones.size(); + vector simulatedTOA(K); + for (size_t i = 0; i < K; i++) { + simulatedTOA[i] = kTimeOfArrival(event, microphones[i]); + } + return simulatedTOA; +} + +/* ************************************************************************* */ +// Simulate time-of-arrival measurements for an entire trajectory +vector> simulateTOA(const vector& microphones, + const vector& trajectory) { + vector> simulatedTOA; + for (auto event : trajectory) { + simulatedTOA.push_back(simulateTOA(microphones, event)); + } + return simulatedTOA; +} + +/* ************************************************************************* */ +// create factor graph +NonlinearFactorGraph createGraph(const vector& microphones, + const vector>& simulatedTOA) { + NonlinearFactorGraph graph; + + // Create a noise model for the TOA error + auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms); + + size_t K = microphones.size(); + size_t key = 0; + for (auto toa : simulatedTOA) { + for (size_t i = 0; i < K; i++) { + graph.emplace_shared(key, microphones[i], toa[i], model); + } + key += 1; + } + return graph; +} + +/* ************************************************************************* */ +// create initial estimate for n events +Values createInitialEstimate(size_t n) { + Values initial; + + Event zero; + for (size_t key = 0; key < n; key++) { + initial.insert(key, zero); + } + return initial; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + // Create microphones + auto microphones = defineMicrophones(); + size_t K = microphones.size(); + for (size_t i = 0; i < K; i++) { + cout << "mic" << i << " = " << microphones[i] << endl; + } + + // Create a ground truth trajectory + const size_t n = 5; + auto groundTruth = createTrajectory(n); + + // Simulate time-of-arrival measurements + auto simulatedTOA = simulateTOA(microphones, groundTruth); + for (size_t key = 0; key < n; key++) { + for (size_t i = 0; i < K; i++) { + cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms" + << endl; + } + } + + // Create factor graph + auto graph = createGraph(microphones, simulatedTOA); + + // Create initial estimate + auto initialEstimate = createInitialEstimate(n); + initialEstimate.print("Initial Estimate:\n"); + + // Optimize using Levenberg-Marquardt optimization. + LevenbergMarquardtParams params; + params.setAbsoluteErrorTol(1e-10); + params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index c503514a69..45a24c1019 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -24,15 +24,16 @@ namespace gtsam { /* ************************************************************************* */ void Event::print(const std::string& s) const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); + std::cout << s << "{'time':" << time_ + << ", 'location': " << location_.transpose() << "}"; } /* ************************************************************************* */ bool Event::equals(const Event& other, double tol) const { - return std::abs(time_ - other.time_) < tol - && traits::Equals(location_, other.location_, tol); + return std::abs(time_ - other.time_) < tol && + traits::Equals(location_, other.location_, tol); } /* ************************************************************************* */ -} //\ namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index fc186857fe..383c349150 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -20,42 +20,43 @@ #pragma once #include +#include + #include #include -#include +#include namespace gtsam { -/// A space-time event +/** + * A space-time event models an event that happens at a certain 3D location, at + * a certain time. One use for it is in sound-based or UWB-ranging tracking or + * SLAM, where we have "time of arrival" measurements at a set of sensors. The + * TOA functor below provides a measurement function for those applications. + */ class Event { + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated - double time_; ///< Time event was generated - Point3 location_; ///< Location at time event was generated - -public: + public: enum { dimension = 4 }; /// Default Constructor - Event() : - time_(0), location_(0,0,0) { - } + Event() : time_(0), location_(0, 0, 0) {} /// Constructor from time and location - Event(double t, const Point3& p) : - time_(t), location_(p) { - } + Event(double t, const Point3& p) : time_(t), location_(p) {} /// Constructor with doubles - Event(double t, double x, double y, double z) : - time_(t), location_(x, y, z) { - } + Event(double t, double x, double y, double z) + : time_(t), location_(x, y, z) {} - double time() const { return time_;} - Point3 location() const { return location_;} + double time() const { return time_; } + Point3 location() const { return location_; } - // TODO we really have to think of a better way to do linear arguments - double height(OptionalJacobian<1,4> H = boost::none) const { - static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished(); + // TODO(frank) we really have to think of a better way to do linear arguments + double height(OptionalJacobian<1, 4> H = boost::none) const { + static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished(); if (H) *H = JacobianZ; return location_.z(); } @@ -64,7 +65,8 @@ class Event { GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const; /** equals with an tolerance */ - GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const; + GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, + double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { @@ -73,28 +75,44 @@ class Event { /// Returns inverse retraction inline Vector4 localCoordinates(const Event& q) const { - return Vector4::Zero(); // TODO + return Vector4::Zero(); // TODO(frank) implement! } +}; - /// Time of arrival to given microphone - double toa(const Point3& microphone, // - OptionalJacobian<1, 4> H1 = boost::none, // - OptionalJacobian<1, 3> H2 = boost::none) const { - static const double Speed = 330; +// Define GTSAM traits +template <> +struct traits : internal::Manifold {}; + +/// Time of arrival to given sensor +class TimeOfArrival { + const double speed_; ///< signal speed + + public: + typedef double result_type; + + /// Constructor with optional speed of signal, in m/sec + explicit TimeOfArrival(double speed = 330) : speed_(speed) {} + + /// Calculate time of arrival + double measure(const Event& event, const Point3& sensor) const { + double distance = gtsam::distance3(event.location(), sensor); + return event.time() + distance / speed_; + } + + /// Calculate time of arrival, with derivatives + double operator()(const Event& event, const Point3& sensor, // + OptionalJacobian<1, 4> H1 = boost::none, // + OptionalJacobian<1, 3> H2 = boost::none) const { Matrix13 D1, D2; - double distance = gtsam::distance3(location_, microphone, D1, D2); + double distance = gtsam::distance3(event.location(), sensor, D1, D2); if (H1) // derivative of toa with respect to event - *H1 << 1.0, D1 / Speed; + *H1 << 1.0, D1 / speed_; if (H2) - // derivative of toa with respect to microphone location - *H2 << D2 / Speed; - return time_ + distance / Speed; + // derivative of toa with respect to sensor location + *H2 << D2 / speed_; + return event.time() + distance / speed_; } }; -// Define GTSAM traits -template<> -struct traits : internal::Manifold {}; - -} //\ namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index ec8ca1f4bf..0349f32930 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -17,10 +17,12 @@ * @date December 2014 */ -#include #include #include +#include + #include + #include using namespace std; @@ -30,56 +32,59 @@ using namespace gtsam; static const double ms = 1e-3; static const double cm = 1e-2; typedef Eigen::Matrix Vector1; -static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0(0,0,0); +static const Point3 microphoneAt0(0, 0, 0); + +static const double kSpeedOfSound = 340; +static const TimeOfArrival kToa(kSpeedOfSound); //***************************************************************************** -TEST( Event, Constructor ) { +TEST(Event, Constructor) { const double t = 0; Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); } //***************************************************************************** -TEST( Event, Toa1 ) { +TEST(Event, Toa1) { Event event(0, 1, 0, 0); - double expected = 1. / 330; - EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); + double expected = 1. / kSpeedOfSound; + EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9); } //***************************************************************************** -TEST( Event, Toa2 ) { - double expectedTOA = timeOfEvent + 1. / 330; - EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +TEST(Event, Toa2) { + double expectedTOA = timeOfEvent + 1. / kSpeedOfSound; + EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9); } //************************************************************************* -TEST (Event, Derivatives) { +TEST(Event, Derivatives) { Matrix14 actualH1; Matrix13 actualH2; - exampleEvent.toa(microphoneAt0, actualH1, actualH2); + kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none), + boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none), + boost::bind(kToa, exampleEvent, _1, boost::none, boost::none), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } //***************************************************************************** -TEST( Event, Expression ) { +TEST(Event, Expression) { Key key = 12; Expression event_(key); - Expression knownMicrophone_(microphoneAt0); // constant expression - Expression expression(&Event::toa, event_, knownMicrophone_); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression expression(kToa, event_, knownMicrophone_); Values values; values.insert(key, exampleEvent); - double expectedTOA = timeOfEvent + 1. / 330; + double expectedTOA = timeOfEvent + 1. / kSpeedOfSound; EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); } @@ -97,4 +102,3 @@ int main() { return TestRegistry::runAllTests(tr); } //***************************************************************************** - diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 6ac3389edc..4c62518318 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -9,7 +9,8 @@ #include #include -#include +#include +#include #include @@ -18,7 +19,7 @@ using namespace gtsam; static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); -SimpleCamera level_camera(level_pose, *K); +PinholeCamera level_camera(level_pose, *K); /* ************************************************************************* */ TEST( InvDepthFactor, Project1) { diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index aaeb0854d9..b07b5acd62 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -263,11 +262,10 @@ TEST(Similarity3, Optimization) { Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x', 1); - PriorFactor factor(key, prior, model); // Create graph NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior(key, prior, model); // Create initial estimate with identity transform Values initial; @@ -304,7 +302,6 @@ TEST(Similarity3, Optimization2) { (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); - PriorFactor factor(X(1), prior, model); // Prior ! BetweenFactor b1(X(1), X(2), m1, betweenNoise); BetweenFactor b2(X(2), X(3), m2, betweenNoise); BetweenFactor b3(X(3), X(4), m3, betweenNoise); @@ -313,7 +310,7 @@ TEST(Similarity3, Optimization2) { // Create graph NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior(X(1), prior, model); // Prior ! graph.push_back(b1); graph.push_back(b2); graph.push_back(b3); diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index d77895d862..ef2d16bf0f 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -274,7 +274,7 @@ class SimPolygon2D { }; // Nonlinear factors from gtsam, for our Value types -#include +#include template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { typedef gtsam::RangeFactor RangeFactorRTV; +#include +class Event { + Event(); + Event(double t, const gtsam::Point3& p); + Event(double t, double x, double y, double z); + double time() const; + gtsam::Point3 location() const; + double height() const; + void print(string s) const; +}; + +class TimeOfArrival { + TimeOfArrival(); + TimeOfArrival(double speed); + double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const; +}; + +#include +virtual class TOAFactor : gtsam::NonlinearFactor { + // For now, because of overload issues, we only expose constructor with known sensor coordinates: + TOAFactor(size_t key1, gtsam::Point3 sensor, double measured, + const gtsam::noiseModel::Base* noiseModel); + static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values); +}; #include template diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index fa94dd255c..36a2cacd8e 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -136,7 +136,7 @@ TEST(LinearEquality, error) /* ************************************************************************* */ TEST(LinearEquality, matrices_NULL) { - // Make sure everything works with NULL noise model + // Make sure everything works with nullptr noise model LinearEquality factor(simple::terms, simple::b, 0); Matrix AExpected(3, 9); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index b0a19c6a4a..777e4b2d3f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -39,7 +39,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { const BatchFixedLagSmoother* e = dynamic_cast(&rhs); - return e != NULL && FixedLagSmoother::equals(*e, tol) + return e != nullptr && FixedLagSmoother::equals(*e, tol) && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } @@ -145,7 +145,7 @@ void BatchFixedLagSmoother::removeFactors( } else { // TODO: Throw an error?? cout << "Attempting to remove a factor from slot " << slot - << ", but it is already NULL." << endl; + << ", but it is already nullptr." << endl; } } } @@ -370,7 +370,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor( cout << " " << DefaultKeyFormatter(key); } } else { - cout << " NULL"; + cout << " nullptr"; } cout << " )" << endl; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 97df375d52..758bcfe488 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -38,7 +38,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } @@ -79,7 +79,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 775dccbb05..600baa9f0f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -394,7 +394,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } @@ -408,7 +408,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index a99360ffbe..d29dfe8ca0 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -58,7 +58,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { const IncrementalFixedLagSmoother* e = dynamic_cast(&rhs); - return e != NULL && FixedLagSmoother::equals(*e, tol) + return e != nullptr && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index a4811abd8f..a708c57cc8 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include using namespace std; @@ -84,7 +83,7 @@ TEST( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index ff5a096b0d..1f19c0e8a5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -134,7 +134,7 @@ TEST( ConcurrentBatchFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -184,7 +184,7 @@ TEST( ConcurrentBatchFilter, getLinearizationPoint ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -246,7 +246,7 @@ TEST( ConcurrentBatchFilter, calculateEstimate ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -330,7 +330,7 @@ TEST( ConcurrentBatchFilter, update_multiple ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -380,7 +380,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -403,7 +403,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.addPrior(1, poseInitial, noisePrior); partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); @@ -504,7 +504,7 @@ TEST( ConcurrentBatchFilter, synchronize_1 ) // Insert factors into the filter, but do not marginalize out any variables. // The synchronization should still be empty NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1090,7 +1090,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1121,7 +1121,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1145,7 +1145,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1176,7 +1176,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1200,8 +1200,8 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1233,7 +1233,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1253,7 +1253,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1286,7 +1286,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 47002acb69..ae9a3f827e 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -104,7 +104,7 @@ TEST( ConcurrentBatchSmoother, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -154,7 +154,7 @@ TEST( ConcurrentBatchSmoother, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -216,7 +216,7 @@ TEST( ConcurrentBatchSmoother, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -302,7 +302,7 @@ TEST( ConcurrentBatchSmoother, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -527,7 +527,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); + smootherFactors.addPrior(4, poseInitial, noisePrior); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -588,7 +588,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) // Add some factors to the smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -617,7 +617,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -642,7 +642,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) // Add some factors to the smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -670,7 +670,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -694,8 +694,8 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -724,7 +724,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -744,7 +744,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -774,7 +774,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 342e2e79f8..c5dba1a694 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -153,7 +153,7 @@ TEST( ConcurrentIncrementalFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -203,7 +203,7 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -259,7 +259,7 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -343,7 +343,7 @@ TEST( ConcurrentIncrementalFilter, update_multiple ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -393,7 +393,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -423,7 +423,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.addPrior(1, poseInitial, noisePrior); partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); @@ -476,7 +476,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -507,7 +507,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.addPrior(1, poseInitial, noisePrior); partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); @@ -605,7 +605,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 ) // Insert factors into the filter, but do not marginalize out any variables. // The synchronization should still be empty NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1192,7 +1192,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1224,7 +1224,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)) // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1251,7 +1251,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1282,7 +1282,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1310,8 +1310,8 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1343,7 +1343,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1367,7 +1367,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1399,7 +1399,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 98b4bf8cc0..5de1150132 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -115,7 +115,7 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -166,7 +166,7 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -223,7 +223,7 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -310,7 +310,7 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -545,7 +545,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); + smootherFactors.addPrior(4, poseInitial, noisePrior); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index d4ebc7c0af..ec78ee6e2c 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -114,7 +114,7 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -165,7 +165,7 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -222,7 +222,7 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -309,7 +309,7 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -546,7 +546,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); + smootherFactors.addPrior(4, poseInitial, noisePrior); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -609,7 +609,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) // Add some factors to the smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -639,7 +639,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 8d99ed4821..3454c352ab 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -82,7 +81,7 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 2240034af5..e0c234b7bd 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -6,7 +6,6 @@ #include #include -#include #include #include #include @@ -27,7 +26,7 @@ NonlinearFactorGraph planarSLAMGraph() { // Prior on pose x1 at the origin. Pose2 prior(0.0, 0.0, 0.0); auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(x1, prior, priorNoise); + graph.addPrior(x1, prior, priorNoise); // Two odometry factors Pose2 odometry(2.0, 0.0, 0.0); diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index af54154693..ce657e7a0c 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -15,7 +15,8 @@ #include #include #include -#include + +#include #include "FindSeparator.h" @@ -83,14 +84,14 @@ namespace gtsam { namespace partition { graph_t *graph; real_t *tpwgts2; ctrl_t *ctrl; - ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); + ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, nullptr, nullptr); ctrl->iptype = METIS_IPTYPE_GROW; - //if () == NULL) + //if () == nullptr) // return METIS_ERROR_INPUT; InitRandom(ctrl->seed); - graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); + graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, nullptr, nullptr); AllocateWorkSpace(ctrl, graph); diff --git a/gtsam_unstable/partition/tests/CMakeLists.txt b/gtsam_unstable/partition/tests/CMakeLists.txt index 2bb95e2439..9e74d99967 100644 --- a/gtsam_unstable/partition/tests/CMakeLists.txt +++ b/gtsam_unstable/partition/tests/CMakeLists.txt @@ -1,2 +1,2 @@ set(ignore_test "testNestedDissection.cpp") -gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis") +gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam") diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index f22de48cfd..35b4677d53 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -77,6 +77,8 @@ class GTSAM_UNSTABLE_EXPORT AHRS { void print(const std::string& s = "") const; virtual ~AHRS(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } /* namespace gtsam */ diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index f2bcb7cd7b..cf56afab26 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -66,7 +66,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 58284c3a69..f499a0f4bc 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -155,7 +155,7 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol && (delta_angles_ - e->delta_angles_).norm() < tol diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index acca233d99..86efd10c11 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -153,7 +153,7 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4 (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol && (delta_angles_ - e->delta_angles_).norm() < tol diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 68c972a868..762199753b 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -81,7 +81,7 @@ class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol); + return e != nullptr && Base::equals(*e, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index c3a67232a6..4763c4263d 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -135,7 +135,7 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5 (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (measurement_acc_ - e->measurement_acc_).norm() < tol && (measurement_gyro_ - e->measurement_gyro_).norm() < tol && (dt_ - e->dt_) < tol diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 3e1263bb9b..5bef97bcf5 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -21,7 +21,7 @@ #pragma once #include -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index c71ee7abd7..c6d892e938 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -100,7 +100,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && + return e != nullptr && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && this->mask_ == e->mask_; } diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 03803b5f4d..bb5835f80c 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -79,7 +79,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL + return e != nullptr && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index d8649a0d51..ce98611605 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -74,7 +74,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL + return e != nullptr && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h new file mode 100644 index 0000000000..ec7da22ef1 --- /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/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 32e8731cd8..b351710415 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p /* ************************************************************************* */ bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; + return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 1df14a8de3..8ad4a14a63 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,33 +17,51 @@ * @date December 2014 */ +#pragma once + #include #include namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) -class TOAFactor: public ExpressionFactor { - +class TOAFactor : public ExpressionFactor { typedef Expression Double_; -public: - + public: /** - * Constructor - * @param some expression yielding an event - * @param microphone_ expression yielding a microphone location - * @param toaMeasurement time of arrival at microphone + * Most general constructor with two expressions + * @param eventExpression expression yielding an event + * @param sensorExpression expression yielding a sensor location + * @param toaMeasurement time of arrival at sensor * @param model noise model + * @param speed optional speed of signal, in m/sec */ TOAFactor(const Expression& eventExpression, - const Expression& microphone_, double toaMeasurement, - const SharedNoiseModel& model) : - ExpressionFactor(model, toaMeasurement, - Double_(&Event::toa, eventExpression, microphone_)) { - } + const Expression& sensorExpression, double toaMeasurement, + const SharedNoiseModel& model, double speed = 330) + : ExpressionFactor( + model, toaMeasurement, + Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {} -}; + /** + * Constructor with fixed sensor + * @param eventExpression expression yielding an event + * @param sensor a known sensor location + * @param toaMeasurement time of arrival at sensor + * @param model noise model + * @param toa optional time of arrival functor + */ + TOAFactor(const Expression& eventExpression, const Point3& sensor, + double toaMeasurement, const SharedNoiseModel& model, + double speed = 330) + : TOAFactor(eventExpression, Expression(sensor), toaMeasurement, + model, speed) {} -} //\ namespace gtsam + static void InsertEvent(Key key, const Event& event, + boost::shared_ptr values) { + values->insert(key, event); + } +}; +} // namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 17c95e6146..8a661f2ef0 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -15,7 +15,7 @@ #include //#include #include -#include +#include #include #include #include @@ -31,20 +31,21 @@ using namespace gtsam; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; -typedef PriorFactor PriorFactorPoint2; -typedef PriorFactor PriorFactorStereoPoint2; -typedef PriorFactor PriorFactorPoint3; -typedef PriorFactor PriorFactorRot2; -typedef PriorFactor PriorFactorRot3; -typedef PriorFactor PriorFactorPose2; -typedef PriorFactor PriorFactorPose3; -typedef PriorFactor PriorFactorCal3_S2; -typedef PriorFactor PriorFactorCal3DS2; -typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; -typedef PriorFactor PriorFactorStereoCamera; +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorPinholeCameraCal3_S2; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -55,29 +56,32 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; -typedef NonlinearEquality NonlinearEqualityPoint2; -typedef NonlinearEquality NonlinearEqualityStereoPoint2; -typedef NonlinearEquality NonlinearEqualityPoint3; -typedef NonlinearEquality NonlinearEqualityRot2; -typedef NonlinearEquality NonlinearEqualityRot3; -typedef NonlinearEquality NonlinearEqualityPose2; -typedef NonlinearEquality NonlinearEqualityPose3; -typedef NonlinearEquality NonlinearEqualityCal3_S2; -typedef NonlinearEquality NonlinearEqualityCal3DS2; -typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef NonlinearEquality NonlinearEqualityStereoCamera; - -typedef RangeFactor RangeFactor2D; -typedef RangeFactor RangeFactor3D; -typedef RangeFactor RangeFactorPose2; -typedef RangeFactor RangeFactorPose3; -typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; +typedef NonlinearEquality NonlinearEqualityStereoCamera; + +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -85,7 +89,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -126,6 +130,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 5bb4dfd2e6..70368cc0e1 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -13,7 +13,6 @@ #include #include -#include using namespace std; @@ -280,8 +279,8 @@ TEST (BetweenFactorEM, updateNoiseModel ) { SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); NonlinearFactorGraph graph; - graph.push_back(gtsam::PriorFactor(key1, p1, model)); - graph.push_back(gtsam::PriorFactor(key2, p2, model)); + graph.addPrior(key1, p1, model); + graph.addPrior(key2, p2, model); f.updateNoiseModels(values, graph); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index a74bfc3c6a..14ad43ae26 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -7,7 +7,7 @@ #include -#include +#include #include #include #include @@ -23,7 +23,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); // camera pose at (0,0,1) looking straight along the x-axis. Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); -SimpleCamera level_camera(level_pose, *K); +PinholeCamera level_camera(level_pose, *K); typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality PoseConstraint; @@ -64,7 +64,7 @@ TEST( InvDepthFactor, optimize) { // add a camera 2 meters to the right Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0)); - SimpleCamera right_camera(right_pose, *K); + PinholeCamera right_camera(right_pose, *K); // projection measurement of landmark into right camera // this measurement disagrees with the depth initialization diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index ca91d4cb59..141a504654 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -16,7 +16,6 @@ * @date Nov 2009 */ -#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h new file mode 100644 index 0000000000..8f8563e9db --- /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/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index dc05711e31..792fd1133a 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include @@ -48,7 +47,7 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.addPrior(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))); graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2)))); return graph; diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 8a6aab6b73..f900b2ffa9 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include using namespace std; @@ -116,8 +115,8 @@ TEST( SmartRangeFactor, optimization ) { graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.emplace_shared >(1, pose1, priorNoise); - graph.emplace_shared >(2, pose2, priorNoise); + graph.addPrior(1, pose1, priorNoise); + graph.addPrior(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp index 432bcb7d49..107defb4c9 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp @@ -198,9 +198,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) { // prior, for the first keyframe: if (kf_id == 0) { - const auto prior = boost::make_shared>( - X(kf_id), Pose3::identity(), priorPoseNoise); - batch_graph.push_back(prior); + batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); } batch_values.insert(X(kf_id), Pose3::identity()); @@ -309,9 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { // prior, for the first keyframe: if (kf_id == 0) { - const auto prior = boost::make_shared>( - X(kf_id), Pose3::identity(), priorPoseNoise); - newFactors.push_back(prior); + newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); } // 2) Run iSAM2: diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 0ac2c24ee1..a0bfc36499 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -335,8 +335,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -396,8 +396,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // add factors NonlinearFactorGraph graph2; - graph2.push_back(PriorFactor(x1, pose1, noisePrior)); - graph2.push_back(PriorFactor(x2, pose2, noisePrior)); + graph2.addPrior(x1, pose1, noisePrior); + graph2.addPrior(x2, pose2, noisePrior); typedef GenericStereoFactor ProjectionFactor; @@ -477,8 +477,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -523,9 +523,9 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam1(cameraPose1, *K); // with camera poses - SimpleCamera cam2(cameraPose2, *K); - SimpleCamera cam3(cameraPose3, *K); + PinholeCamera cam1(cameraPose1, *K); // with camera poses + PinholeCamera cam2(cameraPose2, *K); + PinholeCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // @@ -586,8 +586,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + graph.addPrior(x1, bodyPose1, noisePrior); + graph.addPrior(x2, bodyPose2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -660,8 +660,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -732,8 +732,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -801,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -883,8 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 4b10d5c0b2..042130a249 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,15 +17,16 @@ * @date December 2014 */ -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include -#include #include +#include using namespace std; using namespace gtsam; @@ -43,83 +44,59 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0(0,0,0); +static const Point3 sensorAt0(0, 0, 0); //***************************************************************************** -TEST( TOAFactor, NewWay ) { +TEST(TOAFactor, NewWay) { Key key = 12; - Event_ eventExpression(key); - Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - Double_ expression(&Event::toa, eventExpression, microphoneConstant); - ExpressionFactor factor(model, measurement, expression); + TOAFactor factor(key, sensorAt0, measurement, model); } //***************************************************************************** -TEST( TOAFactor, WholeEnchilada ) { - - static const bool verbose = false; - - // Create microphones +TEST(TOAFactor, WholeEnchilada) { + // Create sensors const double height = 0.5; - vector microphones; - microphones.push_back(Point3(0, 0, height)); - microphones.push_back(Point3(403 * cm, 0, height)); - microphones.push_back(Point3(403 * cm, 403 * cm, height)); - microphones.push_back(Point3(0, 403 * cm, 2 * height)); - EXPECT_LONGS_EQUAL(4, microphones.size()); -// microphones.push_back(Point3(200 * cm, 200 * cm, height)); + vector sensors; + sensors.push_back(Point3(0, 0, height)); + sensors.push_back(Point3(403 * cm, 0, height)); + sensors.push_back(Point3(403 * cm, 403 * cm, height)); + sensors.push_back(Point3(0, 403 * cm, 2 * height)); + EXPECT_LONGS_EQUAL(4, sensors.size()); + // sensors.push_back(Point3(200 * cm, 200 * cm, height)); // Create a ground truth point const double timeOfEvent = 0; Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); // Simulate simulatedTOA - size_t K = microphones.size(); + size_t K = sensors.size(); vector simulatedTOA(K); + TimeOfArrival toa; for (size_t i = 0; i < K; i++) { - simulatedTOA[i] = groundTruthEvent.toa(microphones[i]); - if (verbose) { - cout << "mic" << i << " = " << microphones[i] << endl; - cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; - } + simulatedTOA[i] = toa(groundTruthEvent, sensors[i]); } // Now, estimate using non-linear optimization - ExpressionFactorGraph graph; + NonlinearFactorGraph graph; Key key = 12; - Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { - Point3_ microphone_i(microphones[i]); // constant expression - Double_ predictTOA(&Event::toa, eventExpression, microphone_i); - graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); + graph.emplace_shared(key, sensors[i], simulatedTOA[i], model); } - /// Print the graph - if (verbose) - GTSAM_PRINT(graph); - // Create initial estimate Values initialEstimate; - //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); + // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); Vector4 delta; delta << 0.1, 0.1, -0.1, 0.1; Event estimatedEvent = groundTruthEvent.retract(delta); initialEstimate.insert(key, estimatedEvent); - // Print - if (verbose) - initialEstimate.print("Initial Estimate:\n"); - // Optimize using Levenberg-Marquardt optimization. LevenbergMarquardtParams params; params.setAbsoluteErrorTol(1e-10); - if (verbose) - params.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); - if (verbose) - result.print("Final Result:\n"); EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } @@ -129,4 +106,3 @@ int main() { return TestRegistry::runAllTests(tr); } //***************************************************************************** - diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index d8182ae19a..a7b9136f86 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -16,11 +16,11 @@ * @date Oct 26, 2013 */ +#include #include #include #include -#include #include #include @@ -33,7 +33,6 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -using boost::timer; using boost::format; int main(int argc, char* argv[]) { @@ -61,7 +60,7 @@ int main(int argc, char* argv[]) { cout << "Generating " << nm << " matches" << endl; std::mt19937 rng; std::uniform_int_distribution<> rn(0, N - 1); - + typedef pair Match; vector matches; matches.reserve(nm); @@ -72,46 +71,66 @@ int main(int argc, char* argv[]) { { // DSFBase version - timer tim; + double dsftime = 0; + gttic_(dsftime); DSFBase dsf(N); // Allow for N keys for(const Match& m: matches) dsf.merge(m.first, m.second); - os << tim.elapsed() << ","; - cout << format("DSFBase: %1% s") % tim.elapsed() << endl; + gttoc_(dsftime); + tictoc_getNode(dsftimeNode, dsftime); + dsftime = dsftimeNode->secs(); + os << dsftime << ","; + cout << format("DSFBase: %1% s") % dsftime << endl; + tictoc_reset_(); } { // DSFMap version - timer tim; + double dsftime = 0; + gttic_(dsftime); DSFMap dsf; for(const Match& m: matches) dsf.merge(m.first, m.second); - os << tim.elapsed() << endl; - cout << format("DSFMap: %1% s") % tim.elapsed() << endl; + gttoc_(dsftime); + tictoc_getNode(dsftimeNode, dsftime); + dsftime = dsftimeNode->secs(); + os << dsftime << endl; + cout << format("DSFMap: %1% s") % dsftime << endl; + tictoc_reset_(); } if (false) { // DSF version, functional - timer tim; + double dsftime = 0; + gttic_(dsftime); DSF dsf; for (size_t j = 0; j < N; j++) dsf = dsf.makeSet(j); for(const Match& m: matches) dsf = dsf.makeUnion(m.first, m.second); - os << tim.elapsed() << endl; - cout << format("DSF functional: %1% s") % tim.elapsed() << endl; + gttoc_(dsftime); + tictoc_getNode(dsftimeNode, dsftime); + dsftime = dsftimeNode->secs(); + os << dsftime << endl; + cout << format("DSF functional: %1% s") % dsftime << endl; + tictoc_reset_(); } if (false) { // DSF version, in place - always slower - use functional ! - timer tim; + double dsftime = 0; + gttic_(dsftime); DSF dsf; for (size_t j = 0; j < N; j++) dsf.makeSetInPlace(j); for(const Match& m: matches) dsf.makeUnionInPlace(m.first, m.second); - os << tim.elapsed() << ","; - cout << format("DSF in-place: %1% s") % tim.elapsed() << endl; + gttoc_(dsftime); + tictoc_getNode(dsftimeNode, dsftime); + dsftime = dsftimeNode->secs(); + os << dsftime << ","; + cout << format("DSF in-place: %1% s") % dsftime << endl; + tictoc_reset_(); } } diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 10fd5e1429..035e7e5096 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -94,6 +94,7 @@ % Rot2 - class Rot2, see Doxygen page for details % Rot3 - class Rot3, see Doxygen page for details % SimpleCamera - class SimpleCamera, see Doxygen page for details +% PinholeCameraCal3_S2 - class PinholeCameraCal3_S2, see Doxygen page for details % StereoPoint2 - class StereoPoint2, see Doxygen page for details % %% SLAM and SFM diff --git a/matlab/+gtsam/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m index ab47e92db4..57f9439a4c 100644 --- a/matlab/+gtsam/VisualISAMGenerateData.m +++ b/matlab/+gtsam/VisualISAMGenerateData.m @@ -31,7 +31,7 @@ for i=1:options.nrCameras theta = (i-1)*2*pi/options.nrCameras; t = Point3([r*cos(theta), r*sin(theta), height]'); - truth.cameras{i} = SimpleCamera.Lookat(t, Point3, Point3([0,0,1]'), truth.K); + truth.cameras{i} = PinholeCameraCal3_S2.Lookat(t, Point3, Point3([0,0,1]'), truth.K); % Create measurements for j=1:nrPoints % All landmarks seen in every frame diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 4364e0fe42..51026f6981 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -22,8 +22,8 @@ % rotate data with orientation matrix U and center M data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); n = size(data,2); -x = data(1:n,:)+c(1); -y = data(n+1:2*n,:)+c(2); +x = data(1:n,:)+c(1); +y = data(n+1:2*n,:)+c(2); z = data(2*n+1:end,:)+c(3); % now plot the rotated ellipse diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 71f72f6b96..2b913b52d6 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -13,7 +13,7 @@ import gtsam.* -camera = SimpleCamera(pose, K); +camera = PinholeCameraCal3_S2(pose, K); %% memory allocation cylinderNum = length(cylinders); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 36b7635e26..add2bc75a8 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -92,7 +92,7 @@ cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); end - camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... + camera = PinholeCameraCal3_S2.Lookat(Point3(10, 50, 10), ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.monoK); @@ -154,7 +154,7 @@ %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... % 15, 10]'); - camera = SimpleCamera.Lookat(t, ... + camera = PinholeCameraCal3_S2.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.camera.monoK); cameraPoses{end+1} = camera.pose; diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 7f50f2db86..584ace09ab 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -46,7 +46,7 @@ %% Add Gaussian priors for a pose and a landmark to constrain the system cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); -firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K); +firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K); graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); @@ -60,7 +60,7 @@ initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); - camera_i = SimpleCamera(pose_i, truth.K); + camera_i = PinholeCameraCal3_S2(pose_i, truth.K); initialEstimate.insert(symbol('c',i), camera_i); end for j=1:size(truth.points,2) diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m index d464933286..7116d38383 100644 --- a/matlab/gtsam_tests/testTriangulation.m +++ b/matlab/gtsam_tests/testTriangulation.m @@ -18,11 +18,11 @@ %% Looking along X-axis, 1 meter above ground plane (x-y) upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); pose1 = Pose3(upright, Point3(0, 0, 1)); -camera1 = SimpleCamera(pose1, sharedCal); +camera1 = PinholeCameraCal3_S2(pose1, sharedCal); %% create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); -camera2 = SimpleCamera(pose2, sharedCal); +camera2 = PinholeCameraCal3_S2(pose2, sharedCal); %% landmark ~5 meters infront of camera landmark =Point3 (5, 0.5, 1.2); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 2cceea753c..195b7ff699 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -97,8 +97,8 @@ % Create the camera based on the current pose and the pose of the % camera in the body cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); - camera = SimpleCamera(cameraPose, metadata.camera.calibration); - %camera = SimpleCamera(currentPose, metadata.camera.calibration); + camera = PinholeCameraCal3_S2(cameraPose, metadata.camera.calibration); + %camera = PinholeCameraCal3_S2(currentPose, metadata.camera.calibration); % Record measurements if the landmark is within visual range for j=1:length(metadata.camera.gtLandmarkPoints) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index c9e912ea45..129b8c1761 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -108,7 +108,7 @@ % generate some camera measurements cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i % result for j=1:nrPoints diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index fd4a174693..4557d711fc 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -182,7 +182,7 @@ % generate some camera measurements cam_pose = currentIMUPoseGlobal.compose(actual_transform); % gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i % result for j=1:nrPoints diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 669073e569..79a96209df 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -73,7 +73,7 @@ % generate some camera measurements cam_pose = initial.atPose3(i).compose(actual_transform); gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i for j=1:nrPoints % All landmarks seen in every frame diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 8edcfade7f..ca5b70c62a 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -98,7 +98,7 @@ % generate some camera measurements cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i % result for j=1:nrPoints diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index 629c6d9946..aaccc92482 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -4,7 +4,7 @@ import gtsam.*; - camera = SimpleCamera(pose,K); + camera = PinholeCameraCal3_S2(pose,K); measurements = Values; for i=1:size(landmarks)-1 diff --git a/package_scripts/README.md b/package_scripts/README.md deleted file mode 100644 index 1e33369aa6..0000000000 --- a/package_scripts/README.md +++ /dev/null @@ -1,14 +0,0 @@ - -# How to generate Debian packages - - cd [GTSAM_SOURCE_ROOT] - bash package_scripts/prepare_debian.sh - - -# How to generate Ubuntu packages for a PPA - - cd [GTSAM_SOURCE_ROOT] - bash package_scripts/prepare_ubuntu_pkgs_for_ppa.sh - cd ~/gtsam_ubuntu - bash [GTSAM_SOURCE_ROOT]/upload_all_gtsam_ppa.sh - diff --git a/package_scripts/compile_static_boost.sh b/package_scripts/compile_static_boost.sh deleted file mode 100755 index ca3b99e091..0000000000 --- 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 7b083d8bbc..0000000000 --- a/package_scripts/prepare_debian.sh +++ /dev/null @@ -1,179 +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:" 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 - ;; - ?) - echo "Unknown command line argument!" - exit 1 - ;; - esac -done - -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="Jose Luis Blanco Claraco " 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 589d422fe2..0000000000 --- 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 750fc27b31..0000000000 --- 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 553e1884df..0000000000 --- a/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh +++ /dev/null @@ -1,90 +0,0 @@ -#!/bin/bash -# Creates a set of packages for each different Ubuntu distribution, with the -# intention of uploading them to: -# https://launchpad.net/~joseluisblancoc/ -# -# 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\"" -# - -set -e - -# List of distributions to create PPA packages for: -LST_DISTROS=(xenial bionic disco eoan) - -# 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/" -EMAIL4DEB="Jose Luis Blanco (University of Malaga) " - -# 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 -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="Jose Luis Blanco Claraco " 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 28de2572ac..0000000000 --- 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 c9d113f00a..0000000000 --- a/package_scripts/upload_all_gtsam_ppa.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -find . -name '*.changes' | xargs -I FIL dput ppa:joseluisblancoc/gtsam-develop FIL diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index d9a013a60e..d33c7ba1d2 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index b3e8a34496..d759e83e1b 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -14,7 +14,7 @@ * @author Stephen Williams */ -#include +#include #include #include #include @@ -162,7 +162,7 @@ class NonlinearMotionModel : public NoiseModelFactor2 { /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2()); + return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); } /** @@ -196,7 +196,7 @@ class NonlinearMotionModel : public NoiseModelFactor2 { Vector b = -evaluateError(x1, x2, A1, A2); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained.get() != NULL) { + if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, constrained)); } @@ -292,7 +292,7 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && Base::equals(f); + return (e != nullptr) && Base::equals(f); } /** @@ -325,7 +325,7 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { Vector b = -evaluateError(x1, A1); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained.get() != NULL) { + if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index bf968c8d7a..1f5ec63506 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -401,7 +401,6 @@ TEST(GaussianFactorGraph, hasConstraints) #include #include -#include #include /* ************************************************************************* */ @@ -438,7 +437,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { initValues.insert(l41, Point3(1.61051523, 6.7373052, -0.231582751) ); NonlinearFactorGraph factors; - factors += PriorFactor(xC1, + factors.addPrior(xC1, Pose3(Rot3( -1., 0.0, 1.2246468e-16, 0.0, 1., 0.0, diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 68d10bb7bb..e8e916f046 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -63,7 +62,7 @@ ISAM2 createSlamlikeISAM2( // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -499,7 +498,7 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -669,7 +668,7 @@ namespace { TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -696,7 +695,7 @@ TEST(ISAM2, marginalizeLeaves2) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -726,7 +725,7 @@ TEST(ISAM2, marginalizeLeaves3) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -765,7 +764,7 @@ TEST(ISAM2, marginalizeLeaves4) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 2, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index e877e5a9da..dfdb32b468 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index e45f234aa2..a44a28273e 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -12,7 +12,6 @@ * @date Jun 11, 2012 */ -#include #include #include #include @@ -38,7 +37,7 @@ boost::tuple generateProblem() { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); - graph += PriorFactor(1, priorMean, priorNoise); + graph.addPrior(1, priorMean, priorNoise); // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 7afb4e1785..9d6e58ac72 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -16,7 +16,6 @@ **/ #include -#include #include #include #include @@ -117,7 +116,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); boost::shared_ptr fg = graph.linearize(config); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 3c35c6bc01..d7746e08d5 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -29,7 +29,6 @@ #include // add in headers for specific factors -#include #include #include @@ -53,7 +52,7 @@ TEST(Marginals, planarSLAMmarginals) { // gaussian for prior SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - graph += PriorFactor(x1, priorMean, priorNoise); // add the factor to the graph + graph.addPrior(x1, priorMean, priorNoise); // add the factor to the graph /* add odometry */ // general noisemodel for odometry @@ -195,7 +194,7 @@ TEST(Marginals, planarSLAMmarginals) { /* ************************************************************************* */ TEST(Marginals, order) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(), noiseModel::Unit::Create(3)); + fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3)); fg += BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); fg += BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); fg += BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 95843e5ab4..9d91a344b6 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -16,7 +16,7 @@ #include -#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -527,10 +527,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0)); Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0)); // origin, left camera - SimpleCamera leftCamera(poseLeft, K); + PinholeCamera leftCamera(poseLeft, K); Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right - SimpleCamera rightCamera(poseRight, K); + PinholeCamera rightCamera(poseRight, K); Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6a7a963bc0..fdb080a63b 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -25,8 +25,8 @@ #include #include #include +#include #include -#include #include #include @@ -216,8 +216,8 @@ TEST(testNonlinearFactorGraph, eliminate) { // Priors auto prior = noiseModel::Isotropic::Sigma(3, 1); - graph.add(PriorFactor(11, T11, prior)); - graph.add(PriorFactor(21, T21, prior)); + graph.addPrior(11, T11, prior); + graph.addPrior(21, T21, prior); // Odometry auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3)); @@ -242,6 +242,73 @@ TEST(testNonlinearFactorGraph, eliminate) { EXPECT_LONGS_EQUAL(4, bn->size()); } +/* ************************************************************************* */ +TEST(testNonlinearFactorGraph, addPrior) { + Key k(0); + + // Factor graph. + auto graph = NonlinearFactorGraph(); + + // Add a prior factor for key k. + auto model_double = noiseModel::Isotropic::Sigma(1, 1); + graph.addPrior(k, 10, model_double); + + // Assert the graph has 0 error with the correct values. + Values values; + values.insert(k, 10.0); + EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16); + + // Assert the graph has some error with incorrect values. + values.clear(); + values.insert(k, 11.0); + EXPECT(0 != graph.error(values)); + + // Clear the factor graph and values. + values.clear(); + graph.erase(graph.begin(), graph.end()); + + // Add a Pose3 prior to the factor graph. Use a gaussian noise model by + // providing the covariance matrix. + Eigen::DiagonalMatrix covariance_pose3; + covariance_pose3.setIdentity(); + Pose3 pose{Rot3(), Point3(0, 0, 0)}; + graph.addPrior(k, pose, covariance_pose3); + + // Assert the graph has 0 error with the correct values. + values.insert(k, pose); + EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16); + + // Assert the graph has some error with incorrect values. + values.clear(); + Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)}; + values.insert(k, pose_incorrect); + EXPECT(0 != graph.error(values)); +} + +TEST(NonlinearFactorGraph, printErrors) +{ + const NonlinearFactorGraph fg = createNonlinearFactorGraph(); + const Values c = createValues(); + + // Test that it builds with default parameters. + // We cannot check the output since (at present) output is fixed to std::cout. + fg.printErrors(c); + + // Second round: using callback filter to check that we actually visit all factors: + std::vector visited; + visited.assign(fg.size(), false); + const auto testFilter = + [&](const gtsam::Factor *f, double error, size_t index) { + EXPECT(f!=nullptr); + EXPECT(error>=.0); + visited.at(index)=true; + return false; // do not print + }; + fg.printErrors(c,"Test graph: ", gtsam::DefaultKeyFormatter,testFilter); + + for (bool visit : visited) EXPECT(visit==true); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index c06d10bebc..88ae508b6a 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -116,9 +115,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { // Add a floating landmark constellation if (i == 7) { - new_factors += PriorFactor(lm1, landmark1, model2); - new_factors += PriorFactor(lm2, landmark2, model2); - new_factors += PriorFactor(lm3, landmark3, model2); + new_factors.addPrior(lm1, landmark1, model2); + new_factors.addPrior(lm2, landmark2, model2); + new_factors.addPrior(lm3, landmark3, model2); // Initialize to origin new_init.insert(lm1, Point2(0,0)); @@ -193,9 +192,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // Add a floating landmark constellation if (i == 7) { - new_factors += PriorFactor(lm1, landmark1, model2); - new_factors += PriorFactor(lm2, landmark2, model2); - new_factors += PriorFactor(lm3, landmark3, model2); + new_factors.addPrior(lm1, landmark1, model2); + new_factors.addPrior(lm2, landmark2, model2); + new_factors.addPrior(lm3, landmark3, model2); // Initialize to origin new_init.insert(lm1, Point2(0,0)); @@ -296,8 +295,7 @@ TEST(testNonlinearISAM, loop_closures ) { if (id == 0) { noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001)); - graph.emplace_shared >(Symbol('x', id), - Pose2(0, 0, 0), priorNoise); + graph.addPrior(Symbol('x', id), Pose2(0, 0, 0), priorNoise); } else { isam.update(graph, initialEstimate); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e77935527..a549dc7262 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -16,10 +16,10 @@ */ #include -#include #include #include #include +#include #include #include #include @@ -181,7 +181,7 @@ TEST( NonlinearOptimizer, Factorization ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; @@ -240,8 +240,7 @@ TEST(NonlinearOptimizer, NullFactor) { TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(0, 0, 0), - noiseModel::Isotropic::Sigma(3, 1)); + fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); fg += BetweenFactor(0, 1, Pose2(1, 0, M_PI / 2), noiseModel::Isotropic::Sigma(3, 1)); fg += BetweenFactor(1, 2, Pose2(1, 0, M_PI / 2), @@ -339,31 +338,137 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { } /* ************************************************************************* */ -TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { +TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); - fg += BetweenFactor(0, 1, Pose2(1,0,M_PI/2), + fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); + fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(1, 2, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,0.9,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), noiseModel::Isotropic::Sigma(3,1))); Values init; - init.insert(0, Pose2(10,10,0)); - init.insert(1, Pose2(1,0,M_PI)); - init.insert(2, Pose2(1,1,-M_PI)); + init.insert(0, Pose2(0,0,0)); + init.insert(1, Pose2(0.961187, 0.99965, 1.1781)); Values expected; expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,0,M_PI/2)); - expected.insert(2, Pose2(1,1,M_PI)); + expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); + + LevenbergMarquardtParams lm_params; + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 3e-2)); + EXPECT(assert_equal(expected, lm_result, 3e-2)); + EXPECT(assert_equal(expected, dl_result, 3e-2)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); + fg += BetweenFactor(0, 1, Point2(1,1.8), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,0.9), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,90), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + + Values init; + init.insert(0, Point2(1,1)); + init.insert(1, Point2(1,0)); + + Values expected; + expected.insert(0, Point2(0,0)); + expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize())); - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize())); - EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, lm_result, 1e-4)); + EXPECT(assert_equal(expected, dl_result, 1e-4)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); + fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 11, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 10, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0,9, 0), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + + Values init; + init.insert(0, Pose2(0, 0, 0)); + init.insert(1, Pose2(0, 10, M_PI/4)); + + Values expected; + expected.insert(0, Pose2(0, 0, 0)); + expected.insert(1, Pose2(0, 10, 1.45212)); + + LevenbergMarquardtParams params; + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, lm_result, 1e-1)); + EXPECT(assert_equal(expected, dl_result, 1e-1)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, RobustMeanCalculation) { + + NonlinearFactorGraph fg; + + Values init; + + Values expected; + + auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20), + noiseModel::Isotropic::Sigma(1, 1)); + + vector pts{-10,-3,-1,1,3,10,1000}; + for(auto pt : pts) { + fg.addPrior(0, pt, huber); + } + + init.insert(0, 100.0); + expected.insert(0, 3.33333333); + + DoglegParams params_dl; + params_dl.setRelativeErrorTol(1e-10); + + auto gn_result = GaussNewtonOptimizer(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)); + EXPECT(assert_equal(expected, dl_result, tol)); } /* ************************************************************************* */ @@ -379,9 +484,9 @@ TEST(NonlinearOptimizer, disconnected_graph) { init.insert(X(3), Pose2(0.,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); } @@ -389,23 +494,24 @@ TEST(NonlinearOptimizer, disconnected_graph) { /* ************************************************************************* */ #include -class IterativeLM: public LevenbergMarquardtOptimizer { - +class IterativeLM : public LevenbergMarquardtOptimizer { /// Solver specific parameters ConjugateGradientParameters cgParams_; Values initial_; -public: + public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, - const ConjugateGradientParameters &p, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : - LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) { - } + const ConjugateGradientParameters& p, + const LevenbergMarquardtParams& params = + LevenbergMarquardtParams::LegacyDefaults()) + : LevenbergMarquardtOptimizer(graph, initialValues, params), + cgParams_(p), + initial_(initialValues) {} /// Solve that uses conjugate gradient - virtual VectorValues solve(const GaussianFactorGraph &gfg, - const NonlinearOptimizerParams& params) const { + virtual VectorValues solve(const GaussianFactorGraph& gfg, + const NonlinearOptimizerParams& params) const { VectorValues zeros = initial_.zeroVectors(); return conjugateGradientDescent(gfg, zeros, cgParams_); } @@ -414,19 +520,20 @@ class IterativeLM: public LevenbergMarquardtOptimizer { /* ************************************************************************* */ TEST(NonlinearOptimizer, subclass_solver) { Values expected; - expected.insert(X(1), Pose2(0.,0.,0.)); - expected.insert(X(2), Pose2(1.5,0.,0.)); - expected.insert(X(3), Pose2(3.0,0.,0.)); + expected.insert(X(1), Pose2(0., 0., 0.)); + expected.insert(X(2), Pose2(1.5, 0., 0.)); + expected.insert(X(3), Pose2(3.0, 0., 0.)); Values init; - init.insert(X(1), Pose2(0.,0.,0.)); - init.insert(X(2), Pose2(0.,0.,0.)); - init.insert(X(3), Pose2(0.,0.,0.)); + init.insert(X(1), Pose2(0., 0., 0.)); + init.insert(X(2), Pose2(0., 0., 0.)); + init.insert(X(3), Pose2(0., 0., 0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); + graph += BetweenFactor(X(1), X(2), Pose2(1.5, 0., 0.), + noiseModel::Isotropic::Sigma(3, 1)); + graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); ConjugateGradientParameters p; Values actual = IterativeLM(graph, init, p).optimize(); @@ -481,7 +588,7 @@ struct traits { TEST(NonlinearOptimizer, Traits) { NonlinearFactorGraph fg; - fg += PriorFactor(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); + fg.addPrior(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); Values init; init.insert(0, MyType(0,0,0)); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 66b022c822..9d6cc49ac4 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -126,65 +126,63 @@ TEST( GaussianFactorGraphSystem, multiply_getb) /* ************************************************************************* */ // Test Dummy Preconditioner -TEST( PCGSolver, dummy ) -{ - LevenbergMarquardtParams paramsPCG; - paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; - PCGSolverParameters::shared_ptr pcg = boost::make_shared(); +TEST(PCGSolver, dummy) { + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::Iterative; + auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); - paramsPCG.iterativeParams = pcg; + params.iterativeParams = pcg; NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); - Point2 x0(10,10); + Point2 x0(10, 10); Values c0; c0.insert(X(1), x0); - Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0,fg.error(actualPCG),tol); + DOUBLES_EQUAL(0, fg.error(actualPCG), tol); } /* ************************************************************************* */ // Test Block-Jacobi Precondioner -TEST( PCGSolver, blockjacobi ) -{ - LevenbergMarquardtParams paramsPCG; - paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; - PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - paramsPCG.iterativeParams = pcg; +TEST(PCGSolver, blockjacobi) { + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::Iterative; + auto pcg = boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + params.iterativeParams = pcg; NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); - Point2 x0(10,10); + Point2 x0(10, 10); Values c0; c0.insert(X(1), x0); - Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0,fg.error(actualPCG),tol); + DOUBLES_EQUAL(0, fg.error(actualPCG), tol); } /* ************************************************************************* */ // Test Incremental Subgraph PCG Solver -TEST( PCGSolver, subgraph ) -{ - LevenbergMarquardtParams paramsPCG; - paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; - PCGSolverParameters::shared_ptr pcg = boost::make_shared(); +TEST(PCGSolver, subgraph) { + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::Iterative; + auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); - paramsPCG.iterativeParams = pcg; + params.iterativeParams = pcg; NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); - Point2 x0(10,10); + Point2 x0(10, 10); Values c0; c0.insert(X(1), x0); - Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0,fg.error(actualPCG),tol); + DOUBLES_EQUAL(0, fg.error(actualPCG), tol); } /* ************************************************************************* */ diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 31cf68ebd4..5746022a36 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -26,11 +26,9 @@ #include #include #include -#include using namespace gtsam; -typedef PriorFactor Prior; typedef BetweenFactor Between; typedef NonlinearFactorGraph Graph; @@ -41,7 +39,7 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg += Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); + fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); for(int j=0; j<6; ++j) { truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); @@ -59,4 +57,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 64e111e948..e3252a90bd 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -27,7 +27,7 @@ //#include #include //#include -#include +#include #include #include #include @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include @@ -57,20 +57,21 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; -typedef PriorFactor PriorFactorPoint2; -typedef PriorFactor PriorFactorStereoPoint2; -typedef PriorFactor PriorFactorPoint3; -typedef PriorFactor PriorFactorRot2; -typedef PriorFactor PriorFactorRot3; -typedef PriorFactor PriorFactorPose2; -typedef PriorFactor PriorFactorPose3; -typedef PriorFactor PriorFactorCal3_S2; -typedef PriorFactor PriorFactorCal3DS2; -typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; -typedef PriorFactor PriorFactorStereoCamera; +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorPinholeCameraCal3_S2; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -81,29 +82,32 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; -typedef NonlinearEquality NonlinearEqualityPoint2; -typedef NonlinearEquality NonlinearEqualityStereoPoint2; -typedef NonlinearEquality NonlinearEqualityPoint3; -typedef NonlinearEquality NonlinearEqualityRot2; -typedef NonlinearEquality NonlinearEqualityRot3; -typedef NonlinearEquality NonlinearEqualityPose2; -typedef NonlinearEquality NonlinearEqualityPose3; -typedef NonlinearEquality NonlinearEqualityCal3_S2; -typedef NonlinearEquality NonlinearEqualityCal3DS2; -typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef NonlinearEquality NonlinearEqualityStereoCamera; - -typedef RangeFactor RangeFactor2D; -typedef RangeFactor RangeFactor3D; -typedef RangeFactor RangeFactorPose2; -typedef RangeFactor RangeFactorPose3; -typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; +typedef NonlinearEquality NonlinearEqualityStereoCamera; + +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -111,7 +115,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -158,6 +162,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ @@ -294,7 +299,7 @@ TEST (testSerializationSLAM, factors) { Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0); Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0); CalibratedCamera calibratedCamera(pose3); - SimpleCamera simpleCamera(pose3, cal3_s2); + PinholeCamera simpleCamera(pose3, cal3_s2); StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp new file mode 100644 index 0000000000..5a98c3bf56 --- /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/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 9c63e1aa82..9fedc853bd 100644 --- a/tests/testVisualISAM2.cpp +++ b/tests/testVisualISAM2.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include @@ -59,7 +58,7 @@ TEST(testVisualISAM2, all) // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.emplace_shared>( measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); @@ -78,13 +77,11 @@ TEST(testVisualISAM2, all) static auto kPosePrior = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) .finished()); - graph.emplace_shared>(Symbol('x', 0), poses[0], - kPosePrior); + graph.addPrior(Symbol('x', 0), poses[0], kPosePrior); // Add a prior on landmark l0 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared>(Symbol('l', 0), points[0], - kPointPrior); + graph.addPrior(Symbol('l', 0), points[0], kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp new file mode 100644 index 0000000000..c8bdd8fecc --- /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/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index ec5fc3fa5a..6e0f4ccdf8 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -15,7 +15,6 @@ */ #include -#include #include #include #include @@ -98,7 +97,7 @@ int main(int argc, char *argv[]) { // cout << "Initializing " << 0 << endl; newVariables.insert(0, Pose()); // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); + newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3)); } while(nextMeasurement < measurements.size()) { diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index d0b6b263c7..7aaf37e924 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -51,7 +50,7 @@ int main(int argc, char *argv[]) { // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); - g->add(PriorFactor(0, Pose2(), priorModel)); + g->addPrior(0, Pose2(), priorModel); // LAGO for (size_t i = 0; i < trials; i++) { diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index c3e9f32f4e..e4dec40d04 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include #include using namespace std; @@ -54,7 +54,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { Matrix result; double elapsed; { - boost::timer t; + gttic_(elapsed); if (passDims) for (size_t i=0; isecs(); + tictoc_reset_(); } // delete the matrices for (size_t i=0; isecs(); + tictoc_reset_(); } return elapsed; @@ -126,10 +135,15 @@ double timeVScaleRow(size_t m, size_t n, size_t reps) { double elapsed; Matrix result; { - boost::timer t; + gttic_(elapsed); + for (size_t i=0; isecs(); + tictoc_reset_(); } return elapsed; @@ -156,12 +170,17 @@ double timeColumn(size_t reps) { double elapsed; Vector result; { - boost::timer t; + gttic_(elapsed); + for (size_t i=0; i(M, j); result = column(M, j); - elapsed = t.elapsed(); + + gttoc_(elapsed); + tictoc_getNode(elapsedNode, elapsed); + elapsed = elapsedNode->secs(); + tictoc_reset_(); } return elapsed; } @@ -198,12 +217,17 @@ double timeHouseholder(size_t reps) { // perform timing double elapsed; { - boost::timer t; + gttic_(elapsed); + for (size_t i=0; isecs(); + tictoc_reset_(); } return elapsed; } @@ -222,13 +246,18 @@ double timeMatrixInsert(size_t reps) { // perform timing double elapsed; { - boost::timer t; + gttic_(elapsed); + Matrix big = bigBase; for (size_t rep=0; repsecs(); + tictoc_reset_(); } return elapsed; } diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index f16be40f6a..d9f2ffaf65 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -17,10 +17,10 @@ */ #include -#include #include #include +#include #include #include @@ -30,7 +30,6 @@ using namespace std; //namespace ublas = boost::numeric::ublas; //using namespace Eigen; -using boost::timer; using boost::format; using namespace boost::lambda; @@ -68,7 +67,6 @@ int main(int argc, char* argv[]) { cout << endl; { - timer tim; double basicTime, fullTime, topTime, blockTime; cout << "Row-major matrix, row-major assignment:" << endl; @@ -79,43 +77,54 @@ int main(int argc, char* argv[]) { for(size_t j=0; j<(size_t)mat.cols(); ++j) mat(i,j) = rng(); - tim.restart(); + gttic_(basicTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl; - tim.restart(); + gttic_(fullTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl; - tim.restart(); + gttic_(topTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl; - tim.restart(); + gttic_(blockTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl; cout << endl; } { - timer tim; double basicTime, fullTime, topTime, blockTime; cout << "Row-major matrix, column-major assignment:" << endl; @@ -126,43 +135,54 @@ int main(int argc, char* argv[]) { for(size_t i=0; i<(size_t)mat.rows(); ++i) mat(i,j) = rng(); - tim.restart(); + gttic_(basicTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl; - tim.restart(); + gttic_(fullTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl; - tim.restart(); + gttic_(topTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl; - tim.restart(); + gttic_(blockTime); for(size_t rep=0; repsecs(); + gtsam::tictoc_reset_(); cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl; cout << endl; } { - timer tim; double basicTime, fullTime, topTime, blockTime; typedef pair ij_t; vector ijs(100000); @@ -174,28 +194,44 @@ int main(int argc, char* argv[]) { for(size_t rep=0; rep<1000; ++rep) for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } + gttic_(basicTime); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } - basicTime = tim.elapsed(); + gttoc_(basicTime); + tictoc_getNode(basicTimeNode, basicTime); + basicTime = basicTimeNode->secs(); + gtsam::tictoc_reset_(); cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; + gttic_(fullTime); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); } - fullTime = tim.elapsed(); + gttoc_(fullTime); + tictoc_getNode(fullTimeNode, fullTime); + fullTime = fullTimeNode->secs(); + gtsam::tictoc_reset_(); cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; + gttic_(topTime); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj())); for(size_t rep=0; rep<1000; ++rep) for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); } - topTime = tim.elapsed(); + gttoc_(topTime); + tictoc_getNode(topTimeNode, topTime); + topTime = topTimeNode->secs(); + gtsam::tictoc_reset_(); cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; + gttic_(blockTime); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols())); for(size_t rep=0; rep<1000; ++rep) for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); } - blockTime = tim.elapsed(); + gttoc_(blockTime); + tictoc_getNode(blockTimeNode, blockTime); + blockTime = blockTimeNode->secs(); + gtsam::tictoc_reset_(); cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl; cout << endl; diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index 7a4413ac7a..a056bde246 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -61,7 +60,7 @@ int main(int argc, char *argv[]) { gttic_(Create_measurements); if(step == 0) { // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); + newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3)); newVariables.insert(0, Pose()); } else { Vector eta = Vector::Random(3) * 0.1; diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index ac7a35fcd3..c04a44edb2 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -34,12 +34,12 @@ file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE set_target_properties(wrap_lib wrap PROPERTIES FOLDER "${relative_path}") # Install wrap binary and export target -install(TARGETS wrap EXPORT GTSAM-exports DESTINATION bin) +install(TARGETS wrap EXPORT GTSAM-exports DESTINATION ${CMAKE_INSTALL_BINDIR}) list(APPEND GTSAM_EXPORTED_TARGETS wrap) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) # Install matlab header -install(FILES matlab.h DESTINATION include/wrap) +install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wrap) # Build tests add_subdirectory(tests) diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 9742733f77..099cefa703 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -54,7 +54,16 @@ struct GlobalFunction: public FullyOverloadedFunction { // function name in Cython pxd std::string pxdName() const { return "pxd_" + pyRename(name_); } // function name in Python pyx - std::string pyxName() const { return pyRename(name_); } + std::string pyxName() const { + std::string result = ""; + for(size_t i=0; i= 1) { + result += (overloads[0].namespaces_[i] + "_"); + } + } + result += pyRename(name_); + return result; + } // emit cython wrapper void emit_cython_pxd(FileWriter& pxdFile) const; diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a7db9e1f6b..ec02dc412b 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 0d9adac5fa..3527840a32 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":