diff --git a/.gitignore b/.gitignore index b3ea282dd..2682a6748 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,11 @@ *.txt.user.6d59f0c /python-build/ *.pydevproject +cython/venv +cython/gtsam.cpp +cython/gtsam.cpython-35m-darwin.so +cython/gtsam.pyx +cython/gtsam.so +cython/gtsam_wrapper.pxd +.vscode +.env diff --git a/CMakeLists.txt b/CMakeLists.txt index 77434d135..d82e31228 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,11 +73,12 @@ option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) +option(GTSAM_INSTALL_CYTHON_TOOLBOX "Enable/Disable installation of Cython toolbox" OFF) +option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON) # Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") +if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") endif() if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") @@ -95,6 +96,10 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS) message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") endif() +if(GTSAM_INSTALL_CYTHON_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS) + message(FATAL_ERROR "GTSAM_INSTALL_CYTHON_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the CYTHON toolbox cannot deal with this yet. Please turn one of the two options off.") +endif() + # Flags for choosing default packaging tools set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") @@ -359,6 +364,11 @@ add_subdirectory(examples) # Build timing add_subdirectory(timing) +# Build gtsam_unstable +if (GTSAM_BUILD_UNSTABLE) + add_subdirectory(gtsam_unstable) +endif(GTSAM_BUILD_UNSTABLE) + # Matlab toolbox if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) @@ -378,10 +388,20 @@ if (GTSAM_BUILD_PYTHON) endif() -# Build gtsam_unstable -if (GTSAM_BUILD_UNSTABLE) - add_subdirectory(gtsam_unstable) -endif(GTSAM_BUILD_UNSTABLE) +# Cython wrap +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() + set(GTSAM_EIGENCY_INSTALL_PATH ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) + add_subdirectory(cython) +else() + set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h +endif() + # Install config and export files GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") @@ -509,6 +529,10 @@ endif() if(GTSAM_BUILD_PYTHON) message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") endif() + +message(STATUS "Cython toolbox flags ") +print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") +print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") message(STATUS "===============================================================") # Print warnings at the end diff --git a/LICENSE b/LICENSE index e7424bbc2..e1c3be202 100644 --- a/LICENSE +++ b/LICENSE @@ -4,11 +4,11 @@ LICENSE.BSD in this directory. GTSAM contains two third party libraries, with documentation of licensing and modifications as follows: -- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree +- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree ordering library - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig - - http://www.cise.ufl.edu/research/sparse - - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt + - http://faculty.cse.tamu.edu/davis/suitesparse.html + - Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt - Eigen 3.2: General C++ matrix and linear algebra library - Modified with 3 patches that have been contributed back to the Eigen team: - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) diff --git a/README.md b/README.md index 88f69dec4..02ed5149c 100644 --- a/README.md +++ b/README.md @@ -62,11 +62,14 @@ If you are using the factor in academic work, please cite the publications above In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. - Additional Information ---------------------- -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. +There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion. + +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, +which support (superfast) automatic differentiation, +can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home). See the [`INSTALL`](INSTALL) file for more detailed installation instructions. diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml new file mode 100644 index 000000000..d41ba7f26 --- /dev/null +++ b/bitbucket-pipelines.yml @@ -0,0 +1,15 @@ +# Built from sample configuration for C++ – Make. +# Check https://confluence.atlassian.com/x/5Q4SMw for more examples. +# ----- +# Our custom docker image from Docker Hub as the build environment. +image: dellaert/ubuntu-boost-tbb-eigen3:bionic + +pipelines: + default: + - step: + script: # Modify the commands below to build your repository. + - mkdir build + - cd build + - cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF .. + - make -j2 + - make -j2 check \ No newline at end of file diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index a6860f205..f2ca9933e 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -18,7 +18,10 @@ install(FILES GtsamMakeConfigFile.cmake GtsamMatlabWrap.cmake GtsamPythonWrap.cmake + GtsamCythonWrap.cmake GtsamTesting.cmake + FindCython.cmake + FindNumPy.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake new file mode 100644 index 000000000..23afb00e6 --- /dev/null +++ b/cmake/FindCython.cmake @@ -0,0 +1,76 @@ +# Modifed from: https://github.com/nest/nest-simulator/blob/master/cmake/FindCython.cmake +# +# Find the Cython compiler. +# +# This code sets the following variables: +# +# CYTHON_FOUND +# CYTHON_PATH +# CYTHON_EXECUTABLE +# CYTHON_VERSION +# +# See also UseCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Use the Cython executable that lives next to the Python executable +# if it is a local installation. +find_package( PythonInterp ) +if ( PYTHONINTERP_FOUND ) + execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" + "import Cython; print Cython.__path__" + RESULT_VARIABLE RESULT + OUTPUT_VARIABLE CYTHON_PATH + OUTPUT_STRIP_TRAILING_WHITESPACE + ) +endif () + +# RESULT=0 means ok +if ( NOT RESULT ) + get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + HINTS ${_python_path} + ) +endif () + +# RESULT=0 means ok +if ( NOT RESULT ) + execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" + "import Cython; print Cython.__version__" + RESULT_VARIABLE RESULT + OUTPUT_VARIABLE CYTHON_VAR_OUTPUT + ERROR_VARIABLE CYTHON_VAR_OUTPUT + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if ( RESULT EQUAL 0 ) + string( REGEX REPLACE ".* ([0-9]+\\.[0-9]+(\\.[0-9]+)?).*" "\\1" + CYTHON_VERSION "${CYTHON_VAR_OUTPUT}" ) + endif () +endif () + +include( FindPackageHandleStandardArgs ) +find_package_handle_standard_args( Cython + FOUND_VAR + CYTHON_FOUND + REQUIRED_VARS + CYTHON_PATH + CYTHON_EXECUTABLE + VERSION_VAR + CYTHON_VERSION + ) + diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index 8fb3be25d..cbe46a908 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -83,10 +83,20 @@ FIND_PATH(MKL_FFTW_INCLUDE_DIR NO_DEFAULT_PATH ) -IF(WIN32) +IF(WIN32 AND MKL_ROOT_DIR) SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}") - - IF (MKL_INCLUDE_DIR MATCHES "10.") + IF(MKL_INCLUDE_DIR MATCHES "2017" OR MKL_INCLUDE_DIR MATCHES "2018") + IF(CMAKE_CL_64) + SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_lapack95_lp64 mkl_blas95_lp64) + ELSE() + SET(MKL_LIBS mkl_core mkl_intel_s mkl_lapack95 mkl_blas95) + ENDIF() + IF(TBB_FOUND AND GTSAM_WITH_TBB) + SET(MKL_LIBS ${MKL_LIBS} mkl_tbb_thread) + ELSE() + SET(MKL_LIBS ${MKL_LIBS} mkl_intel_thread libiomp5md) + ENDIF() + ELSEIF(MKL_INCLUDE_DIR MATCHES "10.") IF(CMAKE_CL_64) SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64) ELSE() @@ -115,7 +125,7 @@ IF(WIN32) ENDIF() ENDFOREACH() SET(MKL_FOUND ON) -ELSE() # UNIX and macOS +ELSEIF(MKL_ROOT_DIR) # UNIX and macOS FIND_LIBRARY(MKL_CORE_LIBRARY mkl_core PATHS diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake index 45b33e3aa..f39d64601 100644 --- a/cmake/FindTBB.cmake +++ b/cmake/FindTBB.cmake @@ -82,6 +82,10 @@ if (WIN32) set(_TBB_COMPILER "vc11") set(TBB_COMPILER "vc11") endif(MSVC11) + if(MSVC14) + set(_TBB_COMPILER "vc14") + set(TBB_COMPILER "vc14") + endif(MSVC14) # Todo: add other Windows compilers such as ICL. if(TBB_ARCHITECTURE) set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE}) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake new file mode 100644 index 000000000..73e2b63e0 --- /dev/null +++ b/cmake/GtsamCythonWrap.cmake @@ -0,0 +1,246 @@ +# Check Cython version, need to be >=0.25.2 +# Unset these cached variables to avoid surprises when the python/cython +# in the current environment are different from the cached! +unset(PYTHON_EXECUTABLE CACHE) +unset(CYTHON_EXECUTABLE CACHE) +find_package(Cython 0.25.2 REQUIRED) + +# User-friendly Cython wrapping and installing function. +# Builds a Cython module from the provided interface_header. +# For example, for the interface header gtsam.h, +# this will build the wrap module 'gtsam'. +# +# Arguments: +# +# interface_header: The relative path to the wrapper interface definition file. +# extra_imports: extra header to import in the Cython pxd file. +# For example, to use Cython gtsam.pxd in your own module, +# use "from gtsam cimport *" +# install_path: destination to install the library +# libs: libraries to link with +# dependencies: Dependencies which need to be built before the wrapper +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}") + 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) + # Set up building of cython module + find_package(PythonLibs 2.7 REQUIRED) + include_directories(${PYTHON_INCLUDE_DIRS}) + find_package(NumPy REQUIRED) + include_directories(${NUMPY_INCLUDE_DIRS}) +endfunction() + +# Convert pyx to cpp by executing cython +# This is the first step to compile cython from the command line +# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html +# +# Arguments: +# - target: The specified target for this step +# - pyx_file: The input pyx_file in full *absolute* path +# - generated_cpp: The output cpp file in full absolute path +# - include_dirs: Directories to include when executing cython +function(pyx_to_cpp target pyx_file generated_cpp include_dirs) + foreach(dir ${include_dirs}) + set(includes_for_cython ${includes_for_cython} -I ${dir}) + endforeach() + + add_custom_command( + OUTPUT ${generated_cpp} + COMMAND + ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp} + VERBATIM) + add_custom_target(${target} ALL DEPENDS ${generated_cpp}) +endfunction() + +# Build the cpp file generated by converting pyx using cython +# This is the second step to compile cython from the command line +# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html +# +# Arguments: +# - target: The specified target for this step +# - cpp_file: The input cpp_file in full *absolute* path +# - output_lib_we: The output lib filename only (without extension) +# - output_dir: The output directory +function(build_cythonized_cpp target cpp_file output_lib_we output_dir) + add_library(${target} MODULE ${cpp_file}) + if(APPLE) + set(link_flags "-undefined dynamic_lookup") + endif() + set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "${link_flags}" + OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir}) +endfunction() + +# Cythonize a pyx from the command line as described at +# http://cython.readthedocs.io/en/latest/src/reference/compilation.html +# Arguments: +# - target: The specified target +# - pyx_file: The input pyx_file in full *absolute* path +# - output_lib_we: The output lib filename only (without extension) +# - output_dir: The output directory +# - include_dirs: Directories to include when executing cython +# - libs: Libraries to link with +# - interface_header: For dependency. Any update in interface header will re-trigger cythonize +function(cythonize target pyx_file output_lib_we output_dir include_dirs libs interface_header dependencies) + get_filename_component(pyx_path "${pyx_file}" DIRECTORY) + get_filename_component(pyx_name "${pyx_file}" NAME_WE) + set(generated_cpp "${output_dir}/${pyx_name}.cpp") + + set_up_required_cython_packages() + pyx_to_cpp(${target}_pyx2cpp ${pyx_file} ${generated_cpp} "${include_dirs}") + + # Late dependency injection, to make sure this gets called whenever the interface header is updated + # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes + add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND) + if (NOT "${dependencies}" STREQUAL "") + add_dependencies(${target}_pyx2cpp "${dependencies}") + endif() + + build_cythonized_cpp(${target} ${generated_cpp} ${output_lib_we} ${output_dir}) + if (NOT "${libs}" STREQUAL "") + target_link_libraries(${target} "${libs}") + endif() + add_dependencies(${target} ${target}_pyx2cpp) +endfunction() + +# Internal function that wraps a library and compiles the wrapper +function(wrap_library_cython interface_header generated_files_path extra_imports libs dependencies) + # Wrap codegen interface + # Extract module path and name from interface header file name + # wrap requires interfacePath to be *absolute* + get_filename_component(interface_header "${interface_header}" ABSOLUTE) + get_filename_component(module_path "${interface_header}" PATH) + get_filename_component(module_name "${interface_header}" NAME_WE) + + # Wrap module to Cython pyx + message(STATUS "Cython wrapper generating ${module_name}.pyx") + set(generated_pyx "${generated_files_path}/${module_name}.pyx") + file(MAKE_DIRECTORY "${generated_files_path}") + add_custom_command( + OUTPUT ${generated_pyx} + DEPENDS ${interface_header} wrap + COMMAND + wrap --cython ${module_path} ${module_name} ${generated_files_path} "${extra_imports}" + VERBATIM + WORKING_DIRECTORY ${generated_files_path}/../) + add_custom_target(cython_wrap_${module_name}_pyx ALL DEPENDS ${generated_pyx}) + if(NOT "${dependencies}" STREQUAL "") + add_dependencies(cython_wrap_${module_name}_pyx ${dependencies}) + endif() + + message(STATUS "Cythonize and build ${module_name}.pyx") + get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) + cythonize(cythonize_${module_name} ${generated_pyx} ${module_name} + ${generated_files_path} "${include_dirs}" "${libs}" ${interface_header} cython_wrap_${module_name}_pyx) + + # distclean + add_custom_target(wrap_${module_name}_cython_distclean + 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. + message(STATUS "Installing Cython Toolbox to ${install_path}") #${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() + # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one + get_filename_component(location "${install_path}" PATH) + get_filename_component(name "${install_path}" NAME) + install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" + 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 +# +# Arguments: +# source_directory: The source directory to be installed. "The last component of each directory +# name is appended to the destination directory but a trailing slash may be +# used to avoid this because it leaves the last component empty." +# (https://cmake.org/cmake/help/v3.3/command/install.html?highlight=install#installing-directories) +# dest_directory: The destination directory to install to. +# patterns: list of file patterns to install +function(install_cython_scripts source_directory dest_directory patterns) + set(patterns_args "") + set(exclude_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() + +endfunction() + diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index a9e04a01a..bdd868665 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -208,7 +208,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex OUTPUT ${generated_cpp_file} DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects} COMMAND - wrap + wrap --matlab ${modulePath} ${moduleName} ${generated_files_path} @@ -219,9 +219,9 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex # Set up building of mex module string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}") string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}") - add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects}) - target_link_libraries(${moduleName}_wrapper ${correctedOtherLibraries}) - set_target_properties(${moduleName}_wrapper PROPERTIES + add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects}) + target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries}) + set_target_properties(${moduleName}_matlab_wrapper PROPERTIES OUTPUT_NAME "${moduleName}_wrapper" PREFIX "" SUFFIX ".${mexModuleExt}" @@ -229,12 +229,12 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex ARCHIVE_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" RUNTIME_OUTPUT_DIRECTORY "${compiled_mex_modules_root}" CLEAN_DIRECT_OUTPUT 1) - set_property(TARGET ${moduleName}_wrapper APPEND_STRING PROPERTY COMPILE_FLAGS " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32") - set_property(TARGET ${moduleName}_wrapper APPEND PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs}) + set_property(TARGET ${moduleName}_matlab_wrapper APPEND_STRING PROPERTY COMPILE_FLAGS " ${extraMexFlagsSpaced} ${mexFlagsSpaced} \"-I${MATLAB_ROOT}/extern/include\" -DMATLAB_MEX_FILE -DMX_COMPAT_32") + set_property(TARGET ${moduleName}_matlab_wrapper APPEND PROPERTY INCLUDE_DIRECTORIES ${extraIncludeDirs}) # Disable build type postfixes for the mex module - we install in different directories for each build type instead foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) - set_target_properties(${moduleName}_wrapper PROPERTIES ${build_type_upper}_POSTFIX "") + set_target_properties(${moduleName}_matlab_wrapper PROPERTIES ${build_type_upper}_POSTFIX "") endforeach() # Set up platform-specific flags if(MSVC) @@ -243,17 +243,17 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex else() set(mxLibPath "${MATLAB_ROOT}/extern/lib/win32/microsoft") endif() - target_link_libraries(${moduleName}_wrapper "${mxLibPath}/libmex.lib" "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib") - set_target_properties(${moduleName}_wrapper PROPERTIES LINK_FLAGS "/export:mexFunction") + target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.lib" "${mxLibPath}/libmx.lib" "${mxLibPath}/libmat.lib") + set_target_properties(${moduleName}_matlab_wrapper PROPERTIES LINK_FLAGS "/export:mexFunction") set_property(SOURCE "${generated_cpp_file}" APPEND PROPERTY COMPILE_FLAGS "/bigobj") elseif(APPLE) set(mxLibPath "${MATLAB_ROOT}/bin/maci64") - target_link_libraries(${moduleName}_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib") + target_link_libraries(${moduleName}_matlab_wrapper "${mxLibPath}/libmex.dylib" "${mxLibPath}/libmx.dylib" "${mxLibPath}/libmat.dylib") endif() # Hacking around output issue with custom command # Deletes generated build folder - add_custom_target(wrap_${moduleName}_distclean + add_custom_target(wrap_${moduleName}_matlab_distclean COMMAND cmake -E remove_directory ${generated_files_path} COMMAND cmake -E remove_directory ${compiled_mex_modules_root}) endfunction() @@ -278,13 +278,13 @@ function(install_wrapped_library_internal interfaceHeader) get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH) get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME) install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING PATTERN "*.m") - install(TARGETS ${moduleName}_wrapper + install(TARGETS ${moduleName}_matlab_wrapper LIBRARY DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" RUNTIME DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}") endforeach() else() install(DIRECTORY "${generated_files_path}/" DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} FILES_MATCHING PATTERN "*.m") - install(TARGETS ${moduleName}_wrapper + install(TARGETS ${moduleName}_matlab_wrapper LIBRARY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH} RUNTIME DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}) endif() diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt new file mode 100644 index 000000000..bc21b91d1 --- /dev/null +++ b/cython/CMakeLists.txt @@ -0,0 +1,40 @@ +# Install cython components +include(GtsamCythonWrap) + +# Create the cython toolbox for the gtsam library +if (GTSAM_INSTALL_CYTHON_TOOLBOX) + # build and include the eigency version of eigency + add_subdirectory(gtsam_eigency) + include_directories(${PROJECT_BINARY_DIR}/cython/gtsam_eigency) + + # wrap gtsam + add_custom_target(gtsam_header DEPENDS "../gtsam.h") + wrap_and_install_library_cython("../gtsam.h" # interface_header + "" # extra imports + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + gtsam # library to link with + "wrap;cythonize_eigency;gtsam;gtsam_header" # dependencies which need to be built before wrapping + ) + + # wrap gtsam_unstable + if(GTSAM_BUILD_UNSTABLE) + add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h") + set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *") + wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header + "from gtsam.gtsam cimport *" # extra imports + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + gtsam_unstable # library to link with + "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping + ) + # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this + file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") + endif() + + # 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.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") + # install scripts and tests + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") + +endif () diff --git a/cython/README.md b/cython/README.md new file mode 100644 index 000000000..368d2a76d --- /dev/null +++ b/cython/README.md @@ -0,0 +1,154 @@ +This is the Cython/Python wrapper around the GTSAM C++ library. + +INSTALL +======= +- This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows: + +```bash + pip install -r /cython/requirements.txt +``` + +- For compatiblity 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 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 + +- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH: +```bash +export PYTHONPATH=$PYTHONPATH: +``` + +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 +``` + +WRITING YOUR OWN SCRIPTS +======================== +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 + 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 + 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 + +- Casting from a base class to a derive class must be done explicitly. +Examples: +```Python + noiseBase = factor.get_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 +============ + - 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?) +☐ inner namespaces ==> inner packages? +☐ Wrap fixed-size Matrices/Vectors? + + +Completed/Cancelled: +===== +✔ Fix Python tests: don't use " import * ": Bad style!!! @done (18-03-17 19:50) +✔ Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files +✔ Wrap unstable @done (18-03-17 15:30) +✔ Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30) + ✔ 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. + ✔ 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. +✔ Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00) +✔ Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30) +✔ CMake install script @done (25-11-16 02:30) +✘ [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy +✘ forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work? +✔ wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00) +✔ Global functions @done (22-11-16 21:00) +✔ [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00) +✔ Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00) +✔ Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00) +✔ Casting from parent and grandparents @done (16-11-16 17:00) +✔ Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00) +✔ Support "print obj" @done (16-11-16 17:00) +✔ methods for FastVector: at, [], ... @done (16-11-16 17:00) +✔ Cython: Key and size_t: traits doesn't exist @done (16-09-12 18:34) +✔ KeyVector, KeyList, KeySet... @done (16-09-13 17:19) +✔ [Nice to have] parse typedef @done (16-09-13 17:19) +✔ ctypedef at correct places @done (16-09-12 18:34) +✔ expand template variable type in constructor/static methods? @done (16-09-12 18:34) +✔ NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20) +✔ Value: no default constructor @done (16-09-13 17:20) +✔ ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25) +✔ Delete duplicate methods in derived class @done (16-09-12 13:38) +✔ Fix return properly @done (16-09-11 17:14) + ✔ handle pair @done (16-09-11 17:14) +✔ Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59) +✔ Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59) +✔ Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22) +✔ Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05) +✘ Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20) +✘ Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28) + +- inference before symbolic/linear +- what's the purpose of "virtual" ?? + +Installation: + ☐ Prerequisite: + - Users create venv and pip install requirements before compiling + - Wrap cython script in gtsam/cython folder + ☐ Install built module into venv? diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in new file mode 100644 index 000000000..7d456023f --- /dev/null +++ b/cython/gtsam/__init__.py.in @@ -0,0 +1,2 @@ +from gtsam import * +${GTSAM_UNSTABLE_IMPORT} diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py new file mode 100644 index 000000000..10bd3de11 --- /dev/null +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -0,0 +1,176 @@ +""" +iSAM2 example with ImuFactor. +Author: Robert Truax (C++), Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import math + +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 + + +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.""" + return np.array([x, y, z], dtype=np.float) + + +def ISAM2_plot(values, fignum=0): + """Plot poses.""" + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plt.cla() + + i = 0 + min_bounds = 0, 0, 0 + max_bounds = 0, 0, 0 + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + gtsam_plot.plot_pose3(fignum, pose_i, 10) + position = pose_i.translation().vector() + min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)] + max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)] + # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0 + i += 1 + + # draw + axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1) + axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1) + axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1) + plt.pause(1) + + +# IMU preintegration parameters +# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis +g = 9.81 +n_gravity = vector3(0, 0, -g) +PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) +I = np.eye(3) +PARAMS.setAccelerometerCovariance(I * 0.1) +PARAMS.setGyroscopeCovariance(I * 0.1) +PARAMS.setIntegrationCovariance(I * 0.1) +PARAMS.setUse2ndOrderCoriolis(False) +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)) + + +def IMU_example(): + """Run iSAM 2 example with IMU factor.""" + + # 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()) + pose_0 = camera.pose() + + # Create the set of ground-truth landmarks and poses + angular_velocity = math.radians(180) # rad/sec + delta_t = 1.0/18 # makes for 10 degrees per step + + angular_velocity_vector = vector3(0, -angular_velocity, 0) + linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) + scenario = gtsam.ConstantTwistScenario( + angular_velocity_vector, linear_velocity_vector, pose_0) + + # Create a factor graph + newgraph = gtsam.NonlinearFactorGraph() + + # Create (incremental) ISAM2 solver + isam = gtsam.ISAM2() + + # Create the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initialEstimate = gtsam.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.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) + + # Add imu priors + biasKey = gtsam.symbol(ord('b'), 0) + biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + biasprior = gtsam.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) + newgraph.push_back(velprior) + initialEstimate.insert(V(0), n_velocity) + + accum = gtsam.PreintegratedImuMeasurements(PARAMS) + + # Simulate poses and imu measurements, adding them to the factor graph + for i in range(80): + t = i * delta_t # simulation time + if i == 0: # First time add two poses + pose_1 = scenario.pose(delta_t) + initialEstimate.insert(X(0), pose_0.compose(DELTA)) + initialEstimate.insert(X(1), pose_1.compose(DELTA)) + elif i >= 2: # Add more poses as necessary + pose_i = scenario.pose(t) + initialEstimate.insert(X(i), pose_i.compose(DELTA)) + + if i > 0: + # Add Bias variables periodically + if i % 5 == 0: + biasKey += 1 + factor = gtsam.BetweenFactorConstantBias( + biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) + newgraph.add(factor) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + + # Predict acceleration and gyro measurements in (actual) body frame + nRb = scenario.rotation(t).matrix() + bRn = np.transpose(nRb) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) + measuredOmega = scenario.omega_b(t) + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) + + # Add Imu Factor + imufac = gtsam.ImuFactor( + X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + newgraph.add(imufac) + + # insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity) + accum.resetIntegration() + + # Incremental solution + isam.update(newgraph, initialEstimate) + result = isam.calculateEstimate() + ISAM2_plot(result) + + # reset + newgraph = gtsam.NonlinearFactorGraph() + initialEstimate.clear() + + +if __name__ == '__main__': + IMU_example() diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py new file mode 100644 index 000000000..5d2190d56 --- /dev/null +++ b/cython/gtsam/examples/OdometryExample.py @@ -0,0 +1,52 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Simple robot motion example, with prior and two odometry measurements +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Create noise models +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# 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) +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) + +# Add odometry factors +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# For simplicity, we will use the same noise model for each odometry factor +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) +print("\nFactor Graph:\n{}".format(graph)) + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) +print("\nInitial Estimate:\n{}".format(initial)) + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +print("\nFinal Result:\n{}".format(result)) diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py new file mode 100644 index 000000000..fb038d043 --- /dev/null +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -0,0 +1,80 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Simple robotics example using odometry measurements and bearing-range (laser) measurements +Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Create noise models +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) + +# Create an empty nonlinear factor graph +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) + +# 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)) + +# Add odometry factors between X1,X2 and X2,X3, respectively +graph.add(gtsam.BetweenFactorPose2( + X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) + +# Add Range-Bearing measurements to two different landmarks L1 and L2 +graph.add(gtsam.BearingRangeFactor2D( + X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) +graph.add(gtsam.BearingRangeFactor2D( + X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) +graph.add(gtsam.BearingRangeFactor2D( + X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) + +# Print graph +print("Factor Graph:\n{}".format(graph)) + +# Create (deliberately inaccurate) initial estimate +initial_estimate = gtsam.Values() +initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) +initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) +initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) +initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) +initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) + +# Print +print("Initial Estimate:\n{}".format(initial_estimate)) + +# Optimize using Levenberg-Marquardt optimization. The optimizer +# accepts an optional set of configuration parameters, controlling +# things like convergence criteria, the type of linear system solver +# to use, and the amount of information displayed during optimization. +# Here we will use the default set of parameters. See the +# documentation for the full set of parameters. +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) +result = optimizer.optimize() +print("\nFinal Result:\n{}".format(result)) + +# Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: + print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py new file mode 100644 index 000000000..b15b90864 --- /dev/null +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -0,0 +1,87 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Simple robotics example using odometry measurements and bearing-range (laser) measurements +Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import math + +import numpy as np + +import gtsam + + +def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=np.float) + + +# Create noise models +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + +# 1. Create a factor graph container and add factors to it +graph = gtsam.NonlinearFactorGraph() + +# 2a. Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) +graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + +# 2b. Add odometry factors +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) + +# 2c. Add the loop closure constraint +# This factor encodes the fact that we have returned to the same pose. In real +# systems, 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.add(gtsam.BetweenFactorPose2( + 5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) +print("\nFactor Graph:\n{}".format(graph)) # print + +# 3. Create the data structure to hold the initial_estimate estimate to the +# solution. For illustrative purposes, these have been deliberately set to incorrect values +initial_estimate = gtsam.Values() +initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) +initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) +initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) +print("\nInitial Estimate:\n{}".format(initial_estimate)) # print + +# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer +# The optimizer accepts an optional set of configuration parameters, +# controlling things like convergence criteria, the type of linear +# system solver to use, and the amount of information displayed during +# optimization. We will set a few parameters as a demonstration. +parameters = gtsam.GaussNewtonParams() + +# Stop iterating once the change in error between steps is less than this value +parameters.setRelativeErrorTol(1e-5) +# Do not perform more than N iteration steps +parameters.setMaxIterations(100) +# Create the optimizer ... +optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) +# ... and optimize +result = optimizer.optimize() +print("Final Result:\n{}".format(result)) + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +for i in range(1, 6): + print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md new file mode 100644 index 000000000..067929a20 --- /dev/null +++ b/cython/gtsam/examples/README.md @@ -0,0 +1,4 @@ +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)) diff --git a/cython/gtsam/examples/SFMdata.py b/cython/gtsam/examples/SFMdata.py new file mode 100644 index 000000000..1d5c499fa --- /dev/null +++ b/cython/gtsam/examples/SFMdata.py @@ -0,0 +1,38 @@ +""" +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 +""" +# pylint: disable=invalid-name, E1101 + +import numpy as np + +import gtsam + + +def createPoints(): + # Create the set of ground-truth landmarks + points = [gtsam.Point3(10.0, 10.0, 10.0), + gtsam.Point3(-10.0, 10.0, 10.0), + gtsam.Point3(-10.0, -10.0, 10.0), + gtsam.Point3(10.0, -10.0, 10.0), + gtsam.Point3(10.0, 10.0, -10.0), + gtsam.Point3(-10.0, 10.0, -10.0), + gtsam.Point3(-10.0, -10.0, -10.0), + gtsam.Point3(10.0, -10.0, -10.0)] + return points + + +def createPoses(K): + # Create the set of ground-truth poses + radius = 30.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) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) + poses.append(camera.pose()) + return poses diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py new file mode 100644 index 000000000..e2caee6d4 --- /dev/null +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -0,0 +1,163 @@ +""" +GTSAM Copyright 2010-2018, 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 + +An example of running visual SLAM using iSAM2. +Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +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 +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)) + + +def visual_ISAM2_plot(result): + """ + VisualISAMPlot plots current state of ISAM2 object + Author: Ellon Paiva + Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert + """ + + # Declare an id for the figure + fignum = 0 + + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plt.cla() + + # Plot points + # Can't use data because current frame might not see all points + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) + # gtsam.plot_3d_points(result, [], marginals) + gtsam_plot.plot_3d_points(fignum, result, 'rx') + + # Plot cameras + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + gtsam_plot.plot_pose3(fignum, pose_i, 10) + i += 1 + + # draw + axes.set_xlim3d(-40, 40) + axes.set_ylim3d(-40, 40) + axes.set_zlim3d(-40, 40) + plt.pause(1) + + +def visual_ISAM2_example(): + plt.ion() + + # Define the camera calibration parameters + K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurement_noise = gtsam.noiseModel_Isotropic.Sigma( + 2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses(K) + + # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps + # to maintain proper linearization and efficient variable ordering, iSAM2 + # performs partial relinearization/reordering at each step. A parameter + # structure is available that allows the user to set various properties, such + # as the relinearization threshold and type of linear solver. For this + # example, we we set the relinearization threshold small so the iSAM2 result + # will approach the batch result. + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.01) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + # Create a Factor Graph and Values to hold the new data + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + + # Add factors for each landmark observation + for j, point in enumerate(points): + camera = gtsam.PinholeCameraCal3_S2(pose, K) + measurement = camera.project(point) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2( + measurement, measurement_noise, X(i), L(j), K)) + + # Add an initial guess for the current pose + # Intentionally initialize the variables off from the ground truth + initial_estimate.insert(X(i), pose.compose(gtsam.Pose3( + gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + # 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. + # Also, as iSAM solves incrementally, we must wait until each is observed + # at least twice before adding it to iSAM. + if i == 0: + # Add a prior on pose x0 + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array( + [0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise)) + + # Add a prior on landmark l0 + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + graph.push_back(gtsam.PriorFactorPoint3( + L(0), points[0], point_noise)) # add directly to graph + + # Add initial guesses to all observed landmarks + # Intentionally initialize the variables off from the ground truth + for j, point in enumerate(points): + initial_estimate.insert(L(j), gtsam.Point3( + point.x()-0.25, point.y()+0.20, point.z()+0.15)) + else: + # Update iSAM with the new factors + isam.update(graph, initial_estimate) + # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + # If accuracy is desired at the expense of time, update(*) can be called additional + # times to perform multiple optimizer iterations every step. + isam.update() + current_estimate = isam.calculateEstimate() + print("****************************************************") + print("Frame", i, ":") + for j in range(i + 1): + print(X(j), ":", current_estimate.atPose3(X(j))) + + for j in range(len(points)): + print(L(j), ":", current_estimate.atPoint3(L(j))) + + visual_ISAM2_plot(current_estimate) + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initial_estimate.clear() + + plt.ioff() + plt.show() + + +if __name__ == '__main__': + visual_ISAM2_example() diff --git a/cython/gtsam/examples/__init__.py b/cython/gtsam/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam/tests/__init__.py b/cython/gtsam/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py new file mode 100644 index 000000000..425180173 --- /dev/null +++ b/cython/gtsam/tests/experiments.py @@ -0,0 +1,112 @@ +""" +This file is not a real python unittest. It contains small experiments +to test the wrapper with gtsam_test, a short version of gtsam.h. +Its name convention is different from other tests so it won't be discovered. +""" +import gtsam +import numpy as np + +r = gtsam.Rot3() +print(r) +print(r.pitch()) +r2 = gtsam.Rot3() +r3 = r.compose(r2) +print("r3 pitch:", r3.pitch()) + +v = np.array([1, 1, 1]) +print("v = ", v) +r4 = r3.retract(v) +print("r4 pitch:", r4.pitch()) +r4.print_(b'r4: ') +r3.print_(b"r3: ") + +v = r3.localCoordinates(r4) +print("localCoordinates:", v) + +Rmat = np.array([ + [0.990074, -0.0942928, 0.104218], + [0.104218, 0.990074, -0.0942928], + [-0.0942928, 0.104218, 0.990074] + ]) +r5 = gtsam.Rot3(Rmat) +r5.print_(b"r5: ") + +l = gtsam.Rot3.Logmap(r5) +print("l = ", l) + + +noise = gtsam.noiseModel_Gaussian.Covariance(Rmat) +noise.print_(b"noise:") + +D = np.array([1.,2.,3.]) +diag = gtsam.noiseModel_Diagonal.Variances(D) +print("diag:", diag) +diag.print_(b"diag:") +print("diag R:", diag.R()) + +p = gtsam.Point3() +p.print_("p:") +factor = gtsam.BetweenFactorPoint3(1,2,p, noise) +factor.print_(b"factor:") + +vv = gtsam.VectorValues() +vv.print_(b"vv:") +vv.insert(1, np.array([1.,2.,3.])) +vv.insert(2, np.array([3.,4.])) +vv.insert(3, np.array([5.,6.,7.,8.])) +vv.print_(b"vv:") + +vv2 = gtsam.VectorValues(vv) +vv2.insert(4, np.array([4.,2.,1])) +vv2.print_(b"vv2:") +vv.print_(b"vv:") + +vv.insert(4, np.array([1.,2.,4.])) +vv.print_(b"vv:") +vv3 = vv.add(vv2) + +vv3.print_(b"vv3:") + +values = gtsam.Values() +values.insert(1, gtsam.Point3()) +values.insert(2, gtsam.Rot3()) +values.print_(b"values:") + +factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag) +print "Prior factor vector: ", factor + + + +keys = gtsam.KeyVector() + +keys.push_back(1) +keys.push_back(2) +print 'size: ', keys.size() +print keys.at(0) +print keys.at(1) + +noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) +noise.print_('noise:') +print 'noise print:', noise +f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) +print 'JacobianFactor(7):\n', f +print "A = ", f.getA() +print "b = ", f.getb() + +f = gtsam.JacobianFactor(np.ones(2)) +f.print_('jacoboian b_in:') + + +print "JacobianFactor initalized with b_in:", f + +diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) +fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag) +print "priorfactorvector: ", fv + +print "base noise: ", fv.get_noiseModel() +print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()) + +X = gtsam.symbol(65, 19) +print X +print gtsam.symbolChr(X) +print gtsam.symbolIndex(X) diff --git a/cython/gtsam/tests/gtsam_test.h b/cython/gtsam/tests/gtsam_test.h new file mode 100644 index 000000000..659a7cd0c --- /dev/null +++ b/cython/gtsam/tests/gtsam_test.h @@ -0,0 +1,772 @@ +namespace gtsam { + +#include +typedef size_t Key; + +#include +template class FastVector { + FastVector(); + FastVector(const This& f); + void push_back(const T& e); + //T& operator[](int); + T at(int i); + size_t size() const; +}; + +typedef gtsam::FastVector KeyVector; + +//************************************************************************* +// geometry +//************************************************************************* + +#include +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + //Point2(const gtsam::Point2& l); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& pose, double tol) const; + + // Group + static gtsam::Point2 identity(); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); + //Point3(const gtsam::Point3& l); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point3& p, double tol) const; + + // Group + static gtsam::Point3 identity(); + + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + //Rot2(const gtsam::Rot2& l); + + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + //Rot3(const gtsam::Rot3& l); + + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + 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 Rodrigues(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + Vector quaternion() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Pose2 { + // Standard Constructor + Pose2(); + //Pose2(const gtsam::Pose2& pose); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2& pose, double tol) const; + + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; + + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; + + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Matrix AdjointMap() const; + Vector Adjoint(const Vector& xi) const; + static Matrix wedge(double vx, double vy, double w); + + // Group Actions on Point2 + gtsam::Point2 transform_from(const gtsam::Point2& p) const; + gtsam::Point2 transform_to(const gtsam::Point2& p) const; + + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +class Pose3 { + // Standard Constructors + Pose3(); + //Pose3(const gtsam::Pose3& pose); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) + Pose3(Matrix t); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; + + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& p2) const; + gtsam::Pose3 between(const gtsam::Pose3& p2) const; + + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& T2) const; + + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& p); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + + // Group Action on Point3 + gtsam::Point3 transform_from(const gtsam::Point3& p) const; + gtsam::Point3 transform_to(const gtsam::Point3& p) const; + + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; +}; + +//************************************************************************* +// noise +//************************************************************************* + +namespace noiseModel { +#include +virtual class Base { +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + Matrix R() const; + bool equals(gtsam::noiseModel::Base& expected, double tol); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); + static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + //Null(const gtsam::noiseModel::mEstimator::Null& other); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + //Fair(const gtsam::noiseModel::mEstimator::Fair& other); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + //Huber(const gtsam::noiseModel::mEstimator::Huber& other); + + void print(string s) const; + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + //Tukey(const gtsam::noiseModel::mEstimator::Tukey& other); + + void print(string s) const; + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + //Robust(const gtsam::noiseModel::Robust& other); + + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + //Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + Sampler(int seed); + //Sampler(const gtsam::Sampler& other); + + + //Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); + Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + //JacobianFactor(const gtsam::GaussianFactorGraph& graph); + //JacobianFactor(const gtsam::JacobianFactor& other); + + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + //pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, const Vector& sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + //HessianFactor(const gtsam::GaussianFactorGraph& factors); + //HessianFactor(const gtsam::HessianFactor& other); + + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Values { + Values(); + //Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + + // template + // void insert(size_t j, const T& t); + + // template + // void update(size_t j, const T& t); + void insert(size_t j, const gtsam::Point2& t); + void insert(size_t j, const gtsam::Point3& t); + void insert(size_t j, const gtsam::Rot2& t); + void insert(size_t j, const gtsam::Pose2& t); + void insert(size_t j, const gtsam::Rot3& t); + void insert(size_t j, const gtsam::Pose3& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); + + void update(size_t j, const gtsam::Point2& t); + void update(size_t j, const gtsam::Point3& t); + void update(size_t j, const gtsam::Rot2& t); + void update(size_t j, const gtsam::Pose2& t); + void update(size_t j, const gtsam::Rot3& t); + void update(size_t j, const gtsam::Pose3& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); + + template + T at(size_t j); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; +}; + +#include +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor& other, double tol) const; + + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen +}; + +#include +class NonlinearFactorGraph { + NonlinearFactorGraph(); + //NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + // gtsam::KeySet keys() const; + + // NonlinearFactorGraph + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + //gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + //gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class NoiseModelFactor: gtsam::NonlinearFactor { + void equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below + gtsam::noiseModel::Base* noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +template +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + //PriorFactor(const This& other); + T prior() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + //BetweenFactor(const This& other); + T measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +#include +// Default keyformatter +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); + +#include +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); + +#include +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust(string filename, + gtsam::noiseModel::Base* model); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +pair readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +//************************************************************************* +// Utilities +//************************************************************************* + +namespace utilities { + + #include + // gtsam::KeyList createKeyList(Vector I); + // gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + // gtsam::KeySet createKeySet(Vector I); + // gtsam::KeySet createKeySet(string s, Vector I); + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + gtsam::Values allPose3s(gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + 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 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); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); + +} //\namespace utilities + +#include +class RedirectCout { + RedirectCout(); + string str(); +}; + +} //\namespace gtsam diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py new file mode 100644 index 000000000..3225d2ff9 --- /dev/null +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -0,0 +1,22 @@ +import unittest +import gtsam +import numpy as np + + +class TestCal3Unified(unittest.TestCase): + + def test_Cal3Unified(self): + K = gtsam.Cal3Unified() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fx(), 1.) + + def test_retract(self): + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') + actual = K.retract(d) + self.assertTrue(actual.equals(expected, 1e-9)) + np.testing.assert_allclose(d, K.localCoordinates(actual)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py new file mode 100644 index 000000000..bf63c839b --- /dev/null +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -0,0 +1,78 @@ +import unittest +import gtsam +import numpy as np + +class TestJacobianFactor(unittest.TestCase): + + def test_eliminate(self): + # Recommended way to specify a matrix (see cython/README) + Ax2 = np.array( + [[-5., 0.], + [+0., -5.], + [10., 0.], + [+0., 10.]], order='F') + + # This is good too + Al1 = np.array( + [[5, 0], + [0, 5], + [0, 0], + [0, 0]], dtype=float, order = 'F') + + # Not recommended for performance reasons, but should still work + # as the wrapper should convert it to the correct type and storage order + Ax1 = np.array( + [[0, 0], # f4 + [0, 0], # f4 + [-10, 0], # f2 + [0, -10]]) # f2 + + x2 = 1 + l1 = 2 + x1 = 3 + + # the RHS + b2 = np.array([-1., 1.5, 2., -1.]) + sigmas = np.array([1., 1., 1., 1.]) + model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas) + combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) + + # eliminate the first variable (x2) in the combined factor, destructive + # ! + ord = gtsam.Ordering() + ord.push_back(x2) + actualCG, lf = combined.eliminate(ord) + + # create expected Conditional Gaussian + R11 = np.array([[11.1803, 0.00], + [0.00, 11.1803]]) + S12 = np.array([[-2.23607, 0.00], + [+0.00, -2.23607]]) + S13 = np.array([[-8.94427, 0.00], + [+0.00, -8.94427]]) + d = np.array([2.23607, -1.56525]) + expectedCG = gtsam.GaussianConditional( + x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) + # check if the result matches + self.assertTrue(actualCG.equals(expectedCG, 1e-4)) + + # the expected linear factor + Bl1 = np.array([[4.47214, 0.00], + [0.00, 4.47214]]) + + Bx1 = np.array( + # x1 + [[-4.47214, 0.00], + [+0.00, -4.47214]]) + + # the RHS + b1 = np.array([0.0, 0.894427]) + + model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.])) + expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) + + # check if the result matches the combined (reduced) factor + self.assertTrue(lf.equals(expectedLF, 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py new file mode 100644 index 000000000..56f9e2573 --- /dev/null +++ b/cython/gtsam/tests/test_KalmanFilter.py @@ -0,0 +1,69 @@ +import unittest +import gtsam +import numpy as np + +class TestKalmanFilter(unittest.TestCase): + + def test_KalmanFilter(self): + F = np.eye(2) + B = np.eye(2) + u = np.array([1.0, 0.0]) + modelQ = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + Q = 0.01 * np.eye(2) + H = np.eye(2) + z1 = np.array([1.0, 0.0]) + z2 = np.array([2.0, 0.0]) + z3 = np.array([3.0, 0.0]) + modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + R = 0.01 * np.eye(2) + + # Create the set of expected output TestValues + expected0 = np.array([0.0, 0.0]) + P00 = 0.01 * np.eye(2) + + expected1 = np.array([1.0, 0.0]) + P01 = P00 + Q + I11 = np.linalg.inv(P01) + np.linalg.inv(R) + + expected2 = np.array([2.0, 0.0]) + P12 = np.linalg.inv(I11) + Q + I22 = np.linalg.inv(P12) + np.linalg.inv(R) + + expected3 = np.array([3.0, 0.0]) + P23 = np.linalg.inv(I22) + Q + I33 = np.linalg.inv(P23) + np.linalg.inv(R) + + # Create an KalmanFilter object + KF = gtsam.KalmanFilter(n=2) + + # Create the Kalman Filter initialization point + x_initial = np.array([0.0, 0.0]) + P_initial = 0.01 * np.eye(2) + + # Create an KF object + state = KF.init(x_initial, P_initial) + self.assertTrue(np.allclose(expected0, state.mean())) + self.assertTrue(np.allclose(P00, state.covariance())) + + # Run iteration 1 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected1, state.mean())) + self.assertTrue(np.allclose(P01, state.covariance())) + state = KF.update(state, H, z1, modelR) + self.assertTrue(np.allclose(expected1, state.mean())) + self.assertTrue(np.allclose(I11, state.information())) + + # Run iteration 2 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected2, state.mean())) + state = KF.update(state, H, z2, modelR) + self.assertTrue(np.allclose(expected2, state.mean())) + + # Run iteration 3 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected3, state.mean())) + state = KF.update(state, H, z3, modelR) + self.assertTrue(np.allclose(expected3, state.mean())) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py new file mode 100644 index 000000000..c373f162c --- /dev/null +++ b/cython/gtsam/tests/test_LocalizationExample.py @@ -0,0 +1,50 @@ +import unittest +import gtsam +import numpy as np + +class TestLocalizationExample(unittest.TestCase): + + def test_LocalizationExample(self): + # Create the graph (defined in pose2SLAM.h, derived from + # NonlinearFactorGraph) + graph = gtsam.NonlinearFactorGraph() + + # Add two odometry factors + # create a measurement for both factors (the same in this case) + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta + graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise)) + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) + + # Add three "GPS" measurements + # We use Pose2 Priors here with high variance on theta + groundTruth = gtsam.Values() + groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0)) + groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0)) + groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0)) + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) + for i in range(3): + graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = gtsam.Marginals(graph, result) + P = [None] * result.size() + for i in range(0, result.size()): + pose_i = result.atPose2(i) + self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4)) + P[i] = marginals.marginalCovariance(i) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py new file mode 100644 index 000000000..1100e8334 --- /dev/null +++ b/cython/gtsam/tests/test_OdometryExample.py @@ -0,0 +1,45 @@ +import unittest +import gtsam +import numpy as np + +class TestOdometryExample(unittest.TestCase): + + def test_OdometryExample(self): + # Create the graph (defined in pose2SLAM.h, derived from + # NonlinearFactorGraph) + graph = gtsam.NonlinearFactorGraph() + + # Add a Gaussian prior on pose x_1 + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin + priorNoise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta + # add directly to graph + graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + + # Add two odometry factors + # create a measurement for both factors (the same in this case) + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) + graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + marginals = gtsam.Marginals(graph, result) + marginals.marginalCovariance(1) + + # Check first pose equality + pose_1 = result.atPose2(1) + self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py new file mode 100644 index 000000000..046a93f35 --- /dev/null +++ b/cython/gtsam/tests/test_PlanarSLAMExample.py @@ -0,0 +1,64 @@ +import unittest +import gtsam +from math import pi +import numpy as np + +class TestPose2SLAMExample(unittest.TestCase): + + def test_Pose2SLAMExample(self): + # Assumptions + # - All values are axis aligned + # - Robot poses are facing along the X axis (horizontal, to the right in images) + # - We have full odometry for measurements + # - The robot is on a grid, moving 2 meters each step + + # Create graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + # Add prior + # gaussian for prior + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + # add directly to graph + graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + + # Add odometry + # general noisemodel for odometry + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2( + 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + + # Add pose constraint + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) + initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = gtsam.Marginals(graph, result) + P = marginals.marginalCovariance(1) + + pose_1 = result.atPose2(1) + self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py new file mode 100644 index 000000000..bcaa7be4f --- /dev/null +++ b/cython/gtsam/tests/test_Pose2SLAMExample.py @@ -0,0 +1,62 @@ +import unittest +import gtsam +from math import pi +import numpy as np + +class TestPose2SLAMExample(unittest.TestCase): + + def test_Pose2SLAMExample(self): + # Assumptions + # - All values are axis aligned + # - Robot poses are facing along the X axis (horizontal, to the right in images) + # - We have full odometry for measurements + # - The robot is on a grid, moving 2 meters each step + + # Create graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + # Add prior + # gaussian for prior + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + # add directly to graph + graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + + # Add odometry + # general noisemodel for odometry + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2( + 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + + # Add pose constraint + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) + initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = gtsam.Marginals(graph, result) + P = marginals.marginalCovariance(1) + + pose_1 = result.atPose2(1) + self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py new file mode 100644 index 000000000..8fa50b90c --- /dev/null +++ b/cython/gtsam/tests/test_Pose3.py @@ -0,0 +1,44 @@ +import math +import unittest + +from gtsam import Point3, Rot3, Pose3 + + +class TestPose3(unittest.TestCase): + + def test__between(self): + T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)) + T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) + expected = T2.inverse().compose(T3) + actual = T2.between(T3) + self.assertTrue(actual.equals(expected, 1e-6)) + + def test_transform_to(self): + transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0)) + actual = transform.transform_to(Point3(3,2,10)) + expected = Point3 (2,1,10) + self.assertTrue(actual.equals(expected, 1e-6)) + + def test_range(self): + l1 = Point3(1, 0, 0) + l2 = Point3(1, 1, 0) + x1 = Pose3() + + xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)) + xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + + # establish range is indeed zero + self.assertEqual(1,x1.range(point=l1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0),x1.range(point=l2)) + + # establish range is indeed zero + self.assertEqual(1,x1.range(pose=xl1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py new file mode 100644 index 000000000..e33db2145 --- /dev/null +++ b/cython/gtsam/tests/test_Pose3SLAMExample.py @@ -0,0 +1,46 @@ +import unittest +import numpy as np +import gtsam +from math import pi +from gtsam.utils.circlePose3 import * + +class TestPose3SLAMExample(unittest.TestCase): + + def test_Pose3SLAMExample(self): + # Create a hexagon of poses + hexagon = circlePose3(6, 1.0) + p0 = hexagon.atPose3(0) + p1 = hexagon.atPose3(1) + + # create a Pose graph with one equality constraint and one measurement + fg = gtsam.NonlinearFactorGraph() + fg.add(gtsam.NonlinearEqualityPose3(0, p0)) + delta = p0.between(p1) + covariance = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) + fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance)) + + # Create initial config + initial = gtsam.Values() + s = 0.10 + initial.insert(0, p0) + initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) + initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1))) + initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1))) + initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1))) + initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1))) + + # optimize + optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial) + result = optimizer.optimizeSafely() + + pose_1 = result.atPose3(1) + self.assertTrue(pose_1.equals(p1, 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py new file mode 100644 index 000000000..95ec2ae94 --- /dev/null +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -0,0 +1,25 @@ +import unittest +import gtsam +import numpy as np + +class TestPriorFactor(unittest.TestCase): + + def test_PriorFactor(self): + values = gtsam.Values() + + key = 5 + priorPose3 = gtsam.Pose3() + model = gtsam.noiseModel_Unit.Create(6) + factor = gtsam.PriorFactorPose3(key, priorPose3, model) + values.insert(key, priorPose3) + self.assertEqual(factor.error(values), 0) + + key = 3 + priorVector = np.array([0., 0., 0.]) + model = gtsam.noiseModel_Unit.Create(3) + factor = gtsam.PriorFactorVector(key, priorVector, model) + values.insert(key, priorVector) + self.assertEqual(factor.error(values), 0) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py new file mode 100644 index 000000000..606b26a43 --- /dev/null +++ b/cython/gtsam/tests/test_SFMExample.py @@ -0,0 +1,69 @@ +import unittest +import gtsam +from gtsam import symbol +import numpy as np +import gtsam.utils.visual_data_generator as generator + + +class TestSFMExample(unittest.TestCase): + + def test_SFMExample(self): + options = generator.Options() + options.triangle = False + options.nrCameras = 10 + + [data, truth] = generator.generate_data(options) + + measurementNoiseSigma = 1.0 + pointNoiseSigma = 0.1 + poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) + + graph = gtsam.NonlinearFactorGraph() + + # Add factors for all measurements + measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma) + for i in range(len(data.Z)): + for k in range(len(data.Z[i])): + j = data.J[i][k] + graph.add(gtsam.GenericProjectionFactorCal3_S2( + data.Z[i][k], measurementNoise, + symbol(ord('x'), i), symbol(ord('p'), j), data.K)) + + posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) + graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0), + truth.cameras[0].pose(), posePriorNoise)) + pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma) + graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0), + truth.points[0], pointPriorNoise)) + + # Initial estimate + initialEstimate = gtsam.Values() + for i in range(len(truth.cameras)): + pose_i = truth.cameras[i].pose() + initialEstimate.insert(symbol(ord('x'), i), pose_i) + for j in range(len(truth.points)): + point_j = truth.points[j] + initialEstimate.insert(symbol(ord('p'), j), point_j) + + # Optimization + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + for i in range(5): + optimizer.iterate() + result = optimizer.values() + + # Marginalization + marginals = gtsam.Marginals(graph, result) + marginals.marginalCovariance(symbol(ord('p'), 0)) + marginals.marginalCovariance(symbol(ord('x'), 0)) + + # Check optimized results, should be equal to ground truth + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('p'), j)) + self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py new file mode 100644 index 000000000..4cca1400b --- /dev/null +++ b/cython/gtsam/tests/test_Scenario.py @@ -0,0 +1,36 @@ +import math +import unittest +import numpy as np + +import gtsam + + +class TestScenario(unittest.TestCase): + def setUp(self): + pass + + def test_loop(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + T = 30 + np.testing.assert_almost_equal(W, scenario.omega_b(T)) + np.testing.assert_almost_equal(V, scenario.velocity_b(T)) + np.testing.assert_almost_equal( + np.cross(W, V), scenario.acceleration_b(T)) + + # R = v/w, so test if loop crests at 2*R + R = v / w + T30 = scenario.pose(T) + np.testing.assert_almost_equal( + np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) + self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + + +if __name__ == '__main__': + unittest.main() diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py new file mode 100644 index 000000000..7924a9b1c --- /dev/null +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -0,0 +1,32 @@ +import math +import numpy as np +import unittest + +from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera + +K = Cal3_S2(625, 625, 0, 0, 0) + +class TestSimpleCamera(unittest.TestCase): + + def test_constructor(self): + pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) + camera = SimpleCamera(pose1, K) + self.assertTrue(camera.calibration().equals(K, 1e-9)) + self.assertTrue(camera.pose().equals(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) + + # expected + x = Point3(1,0,0) + y = Point3(0,0,-1) + z = Point3(0,1,0) + wRc = Rot3(x,y,z) + expected = Pose3(wRc,Point3(0.4,0.3,0.1)) + self.assertTrue(camera.pose().equals(expected, 1e-9)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py new file mode 100644 index 000000000..dacd4a116 --- /dev/null +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -0,0 +1,69 @@ +import unittest +import gtsam +from gtsam import symbol +import numpy as np + + +class TestStereoVOExample(unittest.TestCase): + + def test_StereoVOExample(self): + ## Assumptions + # - For simplicity this example is in the camera's coordinate frame + # - X: right, Y: down, Z: forward + # - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis) + # - x1 is fixed with a constraint, x2 is initialized with noisy values + # - No noise on measurements + + ## Create keys for variables + x1 = symbol(ord('x'),1) + x2 = symbol(ord('x'),2) + l1 = symbol(ord('l'),1) + l2 = symbol(ord('l'),2) + l3 = symbol(ord('l'),3) + + ## Create graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + ## add a constraint on the starting pose + first_pose = gtsam.Pose3() + graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose)) + + ## Create realistic calibration and measurement noise model + # format: fx fy skew cx cy baseline + K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) + stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) + + ## Add measurements + # pose 1 + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)) + + #pose 2 + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)) + + ## Create initial estimate for camera poses and landmarks + initialEstimate = gtsam.Values() + initialEstimate.insert(x1, first_pose) + # noisy estimate for pose 2 + initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1))) + expected_l1 = gtsam.Point3( 1, 1, 5) + initialEstimate.insert(l1, expected_l1) + initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5)) + initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5)) + + ## optimize + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimize() + + ## check equality for the first pose and point + pose_x1 = result.atPose3(x1) + self.assertTrue(pose_x1.equals(first_pose,1e-4)) + + point_l1 = result.atPoint3(l1) + self.assertTrue(point_l1.equals(expected_l1,1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py new file mode 100644 index 000000000..08e133840 --- /dev/null +++ b/cython/gtsam/tests/test_Values.py @@ -0,0 +1,69 @@ +import unittest +import numpy as np + +from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3 +from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias + + +class TestValues(unittest.TestCase): + + def test_values(self): + values = Values() + E = EssentialMatrix(Rot3(), Unit3()) + tol = 1e-9 + + values.insert(0, Point2(0,0)) + values.insert(1, Point3(0,0,0)) + values.insert(2, Rot2()) + values.insert(3, Pose2()) + values.insert(4, Rot3()) + values.insert(5, Pose3()) + values.insert(6, Cal3_S2()) + values.insert(7, Cal3DS2()) + values.insert(8, Cal3Bundler()) + values.insert(9, E) + values.insert(10, imuBias_ConstantBias()) + + # Special cases for Vectors and Matrices + # Note that gtsam's Eigen Vectors and Matrices requires double-precision + # floating point numbers in column-major (Fortran style) storage order, + # whereas by default, numpy.array is in row-major order and the type is + # in whatever the number input type is, e.g. np.array([1,2,3]) + # will have 'int' type. + # + # The wrapper will automatically fix the type and storage order for you, + # but for performance reasons, it's recommended to specify the correct + # type and storage order. + vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is + values.insert(11, vec) + mat = np.array([[1., 2.], [3., 4.]], order='F') + values.insert(12, mat) + # Test with dtype int and the default order='C' + # This still works as the wrapper converts to the correct type and order for you + # but is nornally not recommended! + mat2 = np.array([[1,2,],[3,5]]) + values.insert(13, mat2) + + self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) + self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) + self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) + self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) + self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) + self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) + self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) + self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) + self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) + self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) + self.assertTrue(values.atimuBias_ConstantBias( + 10).equals(imuBias_ConstantBias(), tol)) + + # special cases for Vector and Matrix: + actualVector = values.atVector(11) + self.assertTrue(np.allclose(vec, actualVector, tol)) + actualMatrix = values.atMatrix(12) + self.assertTrue(np.allclose(mat, actualMatrix, tol)) + actualMatrix2 = values.atMatrix(13) + self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py new file mode 100644 index 000000000..39bfa6eb4 --- /dev/null +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -0,0 +1,42 @@ +import unittest +import numpy as np +from gtsam import symbol +import gtsam.utils.visual_data_generator as generator +import gtsam.utils.visual_isam as visual_isam + +class TestVisualISAMExample(unittest.TestCase): + + def test_VisualISAMExample(self): + # Data Options + options = generator.Options() + options.triangle = False + options.nrCameras = 20 + + # iSAM Options + isamOptions = visual_isam.Options() + isamOptions.hardConstraint = False + isamOptions.pointPriors = False + isamOptions.batchInitialization = True + isamOptions.reorderInterval = 10 + isamOptions.alwaysRelinearize = False + + # Generate data + data, truth = generator.generate_data(options) + + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, options.nrCameras): + isam, result = visual_isam.step(data, isam, result, truth, currentPose) + + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('l'), j)) + self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/utils/__init__.py b/cython/gtsam/utils/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam/utils/circlePose3.py b/cython/gtsam/utils/circlePose3.py new file mode 100644 index 000000000..7012548f4 --- /dev/null +++ b/cython/gtsam/utils/circlePose3.py @@ -0,0 +1,38 @@ +import gtsam +import numpy as np +from math import pi, cos, sin + + +def circlePose3(numPoses=8, radius=1.0, symbolChar=0): + """ + circlePose3 generates a set of poses in a circle. This function + returns those poses inside a gtsam.Values object, with sequential + keys starting from 0. An optional character may be provided, which + will be stored in the msb of each key (i.e. gtsam.Symbol). + + We use aerospace/navlab convention, X forward, Y right, Z down + First pose will be at (R,0,0) + ^y ^ X + | | + z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) + Vehicle at p0 is looking towards y axis (X-axis points towards world y) + """ + + # Force symbolChar to be a single character + if type(symbolChar) is str: + symbolChar = ord(symbolChar[0]) + + values = gtsam.Values() + theta = 0.0 + dtheta = 2 * pi / numPoses + gRo = gtsam.Rot3( + np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) + for i in range(numPoses): + key = gtsam.symbol(symbolChar, i) + gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0) + oRi = gtsam.Rot3.Yaw( + -theta) # negative yaw goes counterclockwise, with Z down ! + gTi = gtsam.Pose3(gRo.compose(oRi), gti) + values.insert(key, gTi) + theta = theta + dtheta + return values diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py new file mode 100644 index 000000000..19402a080 --- /dev/null +++ b/cython/gtsam/utils/plot.py @@ -0,0 +1,102 @@ +"""Various plotting utlities.""" + +import numpy as np +import matplotlib.pyplot as plt + + +def plot_pose2_on_axes(axes, pose, axis_length=0.1): + """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + t = pose.translation() + origin = np.array([t.x(), t.y()]) + + # draw the camera axes + x_axis = origin + gRp[:, 0] * axis_length + line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], 'r-') + + y_axis = origin + gRp[:, 1] * axis_length + line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], 'g-') + + +def plot_pose2(fignum, pose, axis_length=0.1): + """Plot a 2D pose on given figure with given 'axis_length'.""" + # get figure object + fig = plt.figure(fignum) + axes = fig.gca() + plot_pose2_on_axes(axes, pose, axis_length) + + +def plot_point3_on_axes(axes, point, linespec): + """Plot a 3D point on given axis 'axes' with given 'linespec'.""" + axes.plot([point.x()], [point.y()], [point.z()], linespec) + + +def plot_point3(fignum, point, linespec): + """Plot a 3D point on given figure with given 'linespec'.""" + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plot_point3_on_axes(axes, point, linespec) + + +def plot_3d_points(fignum, values, linespec, marginals=None): + """ + 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. + """ + + keys = values.keys() + + # 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) + 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'.""" + # 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()]) + + # draw the camera axes + x_axis = origin + gRp[:, 0] * axis_length + line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-') + + y_axis = origin + gRp[:, 1] * axis_length + line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-') + + z_axis = origin + gRp[:, 2] * axis_length + line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0) + 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 + + +def plot_pose3(fignum, pose, axis_length=0.1): + """Plot a 3D pose on given figure with given 'axis_length'.""" + # get figure object + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plot_pose3_on_axes(axes, pose, axis_length) diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py new file mode 100644 index 000000000..91194c565 --- /dev/null +++ b/cython/gtsam/utils/visual_data_generator.py @@ -0,0 +1,117 @@ +from __future__ import print_function + +import numpy as np +from math import pi, cos, sin +import gtsam + + +class Options: + """ + Options to generate test scenario + """ + + def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): + """ + Options to generate test scenario + @param triangle: generate a triangle scene with 3 points if True, otherwise + a cube with 8 points + @param nrCameras: number of cameras to generate + @param K: camera calibration object + """ + self.triangle = triangle + self.nrCameras = nrCameras + + +class GroundTruth: + """ + Object holding generated ground-truth data + """ + + def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + self.K = K + self.cameras = [gtsam.Pose3()] * nrCameras + self.points = [gtsam.Point3()] * nrPoints + + def print_(self, s=""): + print(s) + print("K = ", self.K) + print("Cameras: ", len(self.cameras)) + for camera in self.cameras: + print("\t", camera) + print("Points: ", len(self.points)) + for point in self.points: + print("\t", point) + pass + + +class Data: + """ + Object holding generated measurement data + """ + + class NoiseModels: + pass + + def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + self.K = K + self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] + self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] + self.odometry = [gtsam.Pose3()] * nrCameras + + # Set Noise parameters + self.noiseModels = Data.NoiseModels() + self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) + # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( + # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) + self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) + self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) + + +def generate_data(options): + """ Generate ground-truth and measurement data. """ + + K = gtsam.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) + data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints) + + # Generate simulated data + 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) + 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) + ] + + # 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) + # Create measurements + for j in range(nrPoints): + # All landmarks seen in every frame + data.Z[i][j] = truth.cameras[i].project(truth.points[j]) + data.J[i][j] = j + + # Calculate odometry between cameras + for i in range(1, options.nrCameras): + data.odometry[i] = truth.cameras[i - 1].pose().between( + truth.cameras[i].pose()) + + return data, truth diff --git a/cython/gtsam/utils/visual_isam.py b/cython/gtsam/utils/visual_isam.py new file mode 100644 index 000000000..b0ebe68c3 --- /dev/null +++ b/cython/gtsam/utils/visual_isam.py @@ -0,0 +1,131 @@ +import gtsam +from gtsam import symbol + + +class Options: + """ Options for visual isam example. """ + + def __init__(self): + self.hardConstraint = False + self.pointPriors = False + self.batchInitialization = True + self.reorderInterval = 10 + self.alwaysRelinearize = False + + +def initialize(data, truth, options): + # Initialize iSAM + params = gtsam.ISAM2Params() + if options.alwaysRelinearize: + params.setRelinearizeSkip(1) + isam = gtsam.ISAM2(params=params) + + # Add constraints/priors + # TODO: should not be from ground truth! + newFactors = gtsam.NonlinearFactorGraph() + initialEstimates = gtsam.Values() + for i in range(2): + ii = symbol(ord('x'), i) + if i == 0: + if options.hardConstraint: # add hard constraint + newFactors.add( + gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose())) + else: + newFactors.add( + gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(), + data.noiseModels.posePrior)) + initialEstimates.insert(ii, truth.cameras[i].pose()) + + nextPoseIndex = 2 + + # Add visual measurement factors from two first poses and initialize + # observed landmarks + for i in range(2): + ii = symbol(ord('x'), i) + for k in range(len(data.Z[i])): + j = data.J[i][k] + jj = symbol(ord('l'), j) + newFactors.add( + gtsam.GenericProjectionFactorCal3_S2(data.Z[i][ + k], data.noiseModels.measurement, ii, jj, data.K)) + # TODO: initial estimates should not be from ground truth! + if not initialEstimates.exists(jj): + initialEstimates.insert(jj, truth.points[j]) + if options.pointPriors: # add point priors + newFactors.add( + gtsam.PriorFactorPoint3(jj, truth.points[j], + data.noiseModels.pointPrior)) + + # Add odometry between frames 0 and 1 + newFactors.add( + gtsam.BetweenFactorPose3( + symbol(ord('x'), 0), + symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry)) + + # Update ISAM + if options.batchInitialization: # Do a full optimize for first two poses + batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors, + initialEstimates) + fullyOptimized = batchOptimizer.optimize() + isam.update(newFactors, fullyOptimized) + else: + isam.update(newFactors, initialEstimates) + + # figure(1)tic + # t=toc plot(frame_i,t,'r.') tic + result = isam.calculateEstimate() + # t=toc plot(frame_i,t,'g.') + + return isam, result, nextPoseIndex + + +def step(data, isam, result, truth, currPoseIndex): + ''' + Do one step isam update + @param[in] data: measurement data (odometry and visual measurements and their noiseModels) + @param[in/out] isam: current isam object, will be updated + @param[in/out] result: current result object, will be updated + @param[in] truth: ground truth data, used to initialize new variables + @param[currPoseIndex]: index of the current pose + ''' + # iSAM expects us to give it a new set of factors + # along with initial estimates for any new variables introduced. + newFactors = gtsam.NonlinearFactorGraph() + initialEstimates = gtsam.Values() + + # Add odometry + prevPoseIndex = currPoseIndex - 1 + odometry = data.odometry[prevPoseIndex] + newFactors.add( + gtsam.BetweenFactorPose3( + symbol(ord('x'), prevPoseIndex), + symbol(ord('x'), currPoseIndex), odometry, + data.noiseModels.odometry)) + + # Add visual measurement factors and initializations as necessary + for k in range(len(data.Z[currPoseIndex])): + zij = data.Z[currPoseIndex][k] + j = data.J[currPoseIndex][k] + jj = symbol(ord('l'), j) + newFactors.add( + gtsam.GenericProjectionFactorCal3_S2( + zij, data.noiseModels.measurement, + symbol(ord('x'), currPoseIndex), jj, data.K)) + # TODO: initialize with something other than truth + if not result.exists(jj) and not initialEstimates.exists(jj): + lmInit = truth.points[j] + initialEstimates.insert(jj, lmInit) + + # Initial estimates for the new pose. + prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex)) + initialEstimates.insert( + symbol(ord('x'), currPoseIndex), prevPose.compose(odometry)) + + # Update ISAM + # figure(1)tic + isam.update(newFactors, initialEstimates) + # t=toc plot(frame_i,t,'r.') tic + newResult = isam.calculateEstimate() + # t=toc plot(frame_i,t,'g.') + + return isam, newResult diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt new file mode 100644 index 000000000..54b7de9aa --- /dev/null +++ b/cython/gtsam_eigency/CMakeLists.txt @@ -0,0 +1,38 @@ +include(GtsamCythonWrap) + +# Copy eigency's sources to the build folder +# 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(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) + +# include eigency headers +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!!! +cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conversions" + "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "") +cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core" + ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "") +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} + PATTERN "CMakeLists.txt" EXCLUDE + PATTERN "__init__.py.in" EXCLUDE) +install(TARGETS cythonize_eigency_core cythonize_eigency_conversions + DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency") +install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) +configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py) +install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) diff --git a/cython/gtsam_eigency/LICENSE.txt b/cython/gtsam_eigency/LICENSE.txt new file mode 100644 index 000000000..71743c864 --- /dev/null +++ b/cython/gtsam_eigency/LICENSE.txt @@ -0,0 +1,20 @@ +Copyright (c) 2016 Wouter Boomsma + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files (the +"Software"), to deal in the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +The above copyright notice and this permission notice shall be +included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE +LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION +WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/cython/gtsam_eigency/__init__.py.in b/cython/gtsam_eigency/__init__.py.in new file mode 100644 index 000000000..dd278d128 --- /dev/null +++ b/cython/gtsam_eigency/__init__.py.in @@ -0,0 +1,13 @@ +import os +import numpy as np + +__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_PREFIX}" + +def get_includes(include_eigen=True): + root = os.path.dirname(__file__) + parent = os.path.join(root, "..") + path = [root, parent, np.get_include()] + if include_eigen: + path.append(os.path.join(root, __eigen_dir__)) + return path + diff --git a/cython/gtsam_eigency/conversions.pxd b/cython/gtsam_eigency/conversions.pxd new file mode 100644 index 000000000..f4445e585 --- /dev/null +++ b/cython/gtsam_eigency/conversions.pxd @@ -0,0 +1,62 @@ +cimport numpy as np + +cdef api np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) + diff --git a/cython/gtsam_eigency/conversions.pyx b/cython/gtsam_eigency/conversions.pyx new file mode 100644 index 000000000..55c9ae0cd --- /dev/null +++ b/cython/gtsam_eigency/conversions.pyx @@ -0,0 +1,327 @@ +cimport cython +import numpy as np +from numpy.lib.stride_tricks import as_strided + +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[:,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[::1,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[:,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[::1,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[:,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[::1,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[:,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[::1,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[:,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[::1,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[:,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[::1,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[:,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[::1,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[:,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[::1,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[:,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[::1,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[:,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[::1,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[:,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[::1,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[:,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[::1,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[:,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[::1,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[:,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[::1,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[:,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[::1,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[:,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[::1,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[:,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[::1,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[:,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[::1,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[:,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[::1,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[:,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[::1,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + diff --git a/cython/gtsam_eigency/core.pxd b/cython/gtsam_eigency/core.pxd new file mode 100644 index 000000000..9a84c3c16 --- /dev/null +++ b/cython/gtsam_eigency/core.pxd @@ -0,0 +1,917 @@ +cimport cython +cimport numpy as np + +ctypedef signed char schar; +ctypedef unsigned char uchar; + +ctypedef fused dtype: + uchar + schar + short + int + long + float + double + +ctypedef fused DenseType: + Matrix + Array + +ctypedef fused Rows: + _1 + _2 + _3 + _4 + _5 + _6 + _7 + _8 + _9 + _10 + _11 + _12 + _13 + _14 + _15 + _16 + _17 + _18 + _19 + _20 + _21 + _22 + _23 + _24 + _25 + _26 + _27 + _28 + _29 + _30 + _31 + _32 + Dynamic + +ctypedef Rows Cols +ctypedef Rows StrideOuter +ctypedef Rows StrideInner + +ctypedef fused DenseTypeShort: + Vector1i + Vector2i + Vector3i + Vector4i + VectorXi + RowVector1i + RowVector2i + RowVector3i + RowVector4i + RowVectorXi + Matrix1i + Matrix2i + Matrix3i + Matrix4i + MatrixXi + Vector1f + Vector2f + Vector3f + Vector4f + VectorXf + RowVector1f + RowVector2f + RowVector3f + RowVector4f + RowVectorXf + Matrix1f + Matrix2f + Matrix3f + Matrix4f + MatrixXf + Vector1d + Vector2d + Vector3d + Vector4d + VectorXd + RowVector1d + RowVector2d + RowVector3d + RowVector4d + RowVectorXd + Matrix1d + Matrix2d + Matrix3d + Matrix4d + MatrixXd + Vector1cf + Vector2cf + Vector3cf + Vector4cf + VectorXcf + RowVector1cf + RowVector2cf + RowVector3cf + RowVector4cf + RowVectorXcf + Matrix1cf + Matrix2cf + Matrix3cf + Matrix4cf + MatrixXcf + Vector1cd + Vector2cd + Vector3cd + Vector4cd + VectorXcd + RowVector1cd + RowVector2cd + RowVector3cd + RowVector4cd + RowVectorXcd + Matrix1cd + Matrix2cd + Matrix3cd + Matrix4cd + MatrixXcd + Array22i + Array23i + Array24i + Array2Xi + Array32i + Array33i + Array34i + Array3Xi + Array42i + Array43i + Array44i + Array4Xi + ArrayX2i + ArrayX3i + ArrayX4i + ArrayXXi + Array2i + Array3i + Array4i + ArrayXi + Array22f + Array23f + Array24f + Array2Xf + Array32f + Array33f + Array34f + Array3Xf + Array42f + Array43f + Array44f + Array4Xf + ArrayX2f + ArrayX3f + ArrayX4f + ArrayXXf + Array2f + Array3f + Array4f + ArrayXf + Array22d + Array23d + Array24d + Array2Xd + Array32d + Array33d + Array34d + Array3Xd + Array42d + Array43d + Array44d + Array4Xd + ArrayX2d + ArrayX3d + ArrayX4d + ArrayXXd + Array2d + Array3d + Array4d + ArrayXd + Array22cf + Array23cf + Array24cf + Array2Xcf + Array32cf + Array33cf + Array34cf + Array3Xcf + Array42cf + Array43cf + Array44cf + Array4Xcf + ArrayX2cf + ArrayX3cf + ArrayX4cf + ArrayXXcf + Array2cf + Array3cf + Array4cf + ArrayXcf + Array22cd + Array23cd + Array24cd + Array2Xcd + Array32cd + Array33cd + Array34cd + Array3Xcd + Array42cd + Array43cd + Array44cd + Array4Xcd + ArrayX2cd + ArrayX3cd + ArrayX4cd + ArrayXXcd + Array2cd + Array3cd + Array4cd + ArrayXcd + +ctypedef fused StorageOrder: + RowMajor + ColMajor + +ctypedef fused MapOptions: + Aligned + Unaligned + +cdef extern from "eigency_cpp.h" namespace "eigency": + + cdef cppclass _1 "1": + pass + + cdef cppclass _2 "2": + pass + + cdef cppclass _3 "3": + pass + + cdef cppclass _4 "4": + pass + + cdef cppclass _5 "5": + pass + + cdef cppclass _6 "6": + pass + + cdef cppclass _7 "7": + pass + + cdef cppclass _8 "8": + pass + + cdef cppclass _9 "9": + pass + + cdef cppclass _10 "10": + pass + + cdef cppclass _11 "11": + pass + + cdef cppclass _12 "12": + pass + + cdef cppclass _13 "13": + pass + + cdef cppclass _14 "14": + pass + + cdef cppclass _15 "15": + pass + + cdef cppclass _16 "16": + pass + + cdef cppclass _17 "17": + pass + + cdef cppclass _18 "18": + pass + + cdef cppclass _19 "19": + pass + + cdef cppclass _20 "20": + pass + + cdef cppclass _21 "21": + pass + + cdef cppclass _22 "22": + pass + + cdef cppclass _23 "23": + pass + + cdef cppclass _24 "24": + pass + + cdef cppclass _25 "25": + pass + + cdef cppclass _26 "26": + pass + + cdef cppclass _27 "27": + pass + + cdef cppclass _28 "28": + pass + + cdef cppclass _29 "29": + pass + + cdef cppclass _30 "30": + pass + + cdef cppclass _31 "31": + pass + + cdef cppclass _32 "32": + pass + + cdef cppclass PlainObjectBase: + pass + + cdef cppclass Map[DenseTypeShort](PlainObjectBase): + Map() except + + Map(np.ndarray array) except + + + cdef cppclass FlattenedMap[DenseType, dtype, Rows, Cols]: + FlattenedMap() except + + FlattenedMap(np.ndarray array) except + + + cdef cppclass FlattenedMapWithOrder "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder]: + FlattenedMapWithOrder() except + + FlattenedMapWithOrder(np.ndarray array) except + + + cdef cppclass FlattenedMapWithStride "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder, MapOptions, StrideOuter, StrideInner]: + FlattenedMapWithStride() except + + FlattenedMapWithStride(np.ndarray array) except + + + cdef np.ndarray ndarray_view(PlainObjectBase &) + cdef np.ndarray ndarray_copy(PlainObjectBase &) + cdef np.ndarray ndarray(PlainObjectBase &) + + +cdef extern from "eigency_cpp.h" namespace "Eigen": + + cdef cppclass Dynamic: + pass + + cdef cppclass RowMajor: + pass + + cdef cppclass ColMajor: + pass + + cdef cppclass Aligned: + pass + + cdef cppclass Unaligned: + pass + + cdef cppclass Matrix(PlainObjectBase): + pass + + cdef cppclass Array(PlainObjectBase): + pass + + cdef cppclass VectorXd(PlainObjectBase): + pass + + cdef cppclass Vector1i(PlainObjectBase): + pass + + cdef cppclass Vector2i(PlainObjectBase): + pass + + cdef cppclass Vector3i(PlainObjectBase): + pass + + cdef cppclass Vector4i(PlainObjectBase): + pass + + cdef cppclass VectorXi(PlainObjectBase): + pass + + cdef cppclass RowVector1i(PlainObjectBase): + pass + + cdef cppclass RowVector2i(PlainObjectBase): + pass + + cdef cppclass RowVector3i(PlainObjectBase): + pass + + cdef cppclass RowVector4i(PlainObjectBase): + pass + + cdef cppclass RowVectorXi(PlainObjectBase): + pass + + cdef cppclass Matrix1i(PlainObjectBase): + pass + + cdef cppclass Matrix2i(PlainObjectBase): + pass + + cdef cppclass Matrix3i(PlainObjectBase): + pass + + cdef cppclass Matrix4i(PlainObjectBase): + pass + + cdef cppclass MatrixXi(PlainObjectBase): + pass + + cdef cppclass Vector1f(PlainObjectBase): + pass + + cdef cppclass Vector2f(PlainObjectBase): + pass + + cdef cppclass Vector3f(PlainObjectBase): + pass + + cdef cppclass Vector4f(PlainObjectBase): + pass + + cdef cppclass VectorXf(PlainObjectBase): + pass + + cdef cppclass RowVector1f(PlainObjectBase): + pass + + cdef cppclass RowVector2f(PlainObjectBase): + pass + + cdef cppclass RowVector3f(PlainObjectBase): + pass + + cdef cppclass RowVector4f(PlainObjectBase): + pass + + cdef cppclass RowVectorXf(PlainObjectBase): + pass + + cdef cppclass Matrix1f(PlainObjectBase): + pass + + cdef cppclass Matrix2f(PlainObjectBase): + pass + + cdef cppclass Matrix3f(PlainObjectBase): + pass + + cdef cppclass Matrix4f(PlainObjectBase): + pass + + cdef cppclass MatrixXf(PlainObjectBase): + pass + + cdef cppclass Vector1d(PlainObjectBase): + pass + + cdef cppclass Vector2d(PlainObjectBase): + pass + + cdef cppclass Vector3d(PlainObjectBase): + pass + + cdef cppclass Vector4d(PlainObjectBase): + pass + + cdef cppclass VectorXd(PlainObjectBase): + pass + + cdef cppclass RowVector1d(PlainObjectBase): + pass + + cdef cppclass RowVector2d(PlainObjectBase): + pass + + cdef cppclass RowVector3d(PlainObjectBase): + pass + + cdef cppclass RowVector4d(PlainObjectBase): + pass + + cdef cppclass RowVectorXd(PlainObjectBase): + pass + + cdef cppclass Matrix1d(PlainObjectBase): + pass + + cdef cppclass Matrix2d(PlainObjectBase): + pass + + cdef cppclass Matrix3d(PlainObjectBase): + pass + + cdef cppclass Matrix4d(PlainObjectBase): + pass + + cdef cppclass MatrixXd(PlainObjectBase): + pass + + cdef cppclass Vector1cf(PlainObjectBase): + pass + + cdef cppclass Vector2cf(PlainObjectBase): + pass + + cdef cppclass Vector3cf(PlainObjectBase): + pass + + cdef cppclass Vector4cf(PlainObjectBase): + pass + + cdef cppclass VectorXcf(PlainObjectBase): + pass + + cdef cppclass RowVector1cf(PlainObjectBase): + pass + + cdef cppclass RowVector2cf(PlainObjectBase): + pass + + cdef cppclass RowVector3cf(PlainObjectBase): + pass + + cdef cppclass RowVector4cf(PlainObjectBase): + pass + + cdef cppclass RowVectorXcf(PlainObjectBase): + pass + + cdef cppclass Matrix1cf(PlainObjectBase): + pass + + cdef cppclass Matrix2cf(PlainObjectBase): + pass + + cdef cppclass Matrix3cf(PlainObjectBase): + pass + + cdef cppclass Matrix4cf(PlainObjectBase): + pass + + cdef cppclass MatrixXcf(PlainObjectBase): + pass + + cdef cppclass Vector1cd(PlainObjectBase): + pass + + cdef cppclass Vector2cd(PlainObjectBase): + pass + + cdef cppclass Vector3cd(PlainObjectBase): + pass + + cdef cppclass Vector4cd(PlainObjectBase): + pass + + cdef cppclass VectorXcd(PlainObjectBase): + pass + + cdef cppclass RowVector1cd(PlainObjectBase): + pass + + cdef cppclass RowVector2cd(PlainObjectBase): + pass + + cdef cppclass RowVector3cd(PlainObjectBase): + pass + + cdef cppclass RowVector4cd(PlainObjectBase): + pass + + cdef cppclass RowVectorXcd(PlainObjectBase): + pass + + cdef cppclass Matrix1cd(PlainObjectBase): + pass + + cdef cppclass Matrix2cd(PlainObjectBase): + pass + + cdef cppclass Matrix3cd(PlainObjectBase): + pass + + cdef cppclass Matrix4cd(PlainObjectBase): + pass + + cdef cppclass MatrixXcd(PlainObjectBase): + pass + + cdef cppclass Array22i(PlainObjectBase): + pass + + cdef cppclass Array23i(PlainObjectBase): + pass + + cdef cppclass Array24i(PlainObjectBase): + pass + + cdef cppclass Array2Xi(PlainObjectBase): + pass + + cdef cppclass Array32i(PlainObjectBase): + pass + + cdef cppclass Array33i(PlainObjectBase): + pass + + cdef cppclass Array34i(PlainObjectBase): + pass + + cdef cppclass Array3Xi(PlainObjectBase): + pass + + cdef cppclass Array42i(PlainObjectBase): + pass + + cdef cppclass Array43i(PlainObjectBase): + pass + + cdef cppclass Array44i(PlainObjectBase): + pass + + cdef cppclass Array4Xi(PlainObjectBase): + pass + + cdef cppclass ArrayX2i(PlainObjectBase): + pass + + cdef cppclass ArrayX3i(PlainObjectBase): + pass + + cdef cppclass ArrayX4i(PlainObjectBase): + pass + + cdef cppclass ArrayXXi(PlainObjectBase): + pass + + cdef cppclass Array2i(PlainObjectBase): + pass + + cdef cppclass Array3i(PlainObjectBase): + pass + + cdef cppclass Array4i(PlainObjectBase): + pass + + cdef cppclass ArrayXi(PlainObjectBase): + pass + + cdef cppclass Array22f(PlainObjectBase): + pass + + cdef cppclass Array23f(PlainObjectBase): + pass + + cdef cppclass Array24f(PlainObjectBase): + pass + + cdef cppclass Array2Xf(PlainObjectBase): + pass + + cdef cppclass Array32f(PlainObjectBase): + pass + + cdef cppclass Array33f(PlainObjectBase): + pass + + cdef cppclass Array34f(PlainObjectBase): + pass + + cdef cppclass Array3Xf(PlainObjectBase): + pass + + cdef cppclass Array42f(PlainObjectBase): + pass + + cdef cppclass Array43f(PlainObjectBase): + pass + + cdef cppclass Array44f(PlainObjectBase): + pass + + cdef cppclass Array4Xf(PlainObjectBase): + pass + + cdef cppclass ArrayX2f(PlainObjectBase): + pass + + cdef cppclass ArrayX3f(PlainObjectBase): + pass + + cdef cppclass ArrayX4f(PlainObjectBase): + pass + + cdef cppclass ArrayXXf(PlainObjectBase): + pass + + cdef cppclass Array2f(PlainObjectBase): + pass + + cdef cppclass Array3f(PlainObjectBase): + pass + + cdef cppclass Array4f(PlainObjectBase): + pass + + cdef cppclass ArrayXf(PlainObjectBase): + pass + + cdef cppclass Array22d(PlainObjectBase): + pass + + cdef cppclass Array23d(PlainObjectBase): + pass + + cdef cppclass Array24d(PlainObjectBase): + pass + + cdef cppclass Array2Xd(PlainObjectBase): + pass + + cdef cppclass Array32d(PlainObjectBase): + pass + + cdef cppclass Array33d(PlainObjectBase): + pass + + cdef cppclass Array34d(PlainObjectBase): + pass + + cdef cppclass Array3Xd(PlainObjectBase): + pass + + cdef cppclass Array42d(PlainObjectBase): + pass + + cdef cppclass Array43d(PlainObjectBase): + pass + + cdef cppclass Array44d(PlainObjectBase): + pass + + cdef cppclass Array4Xd(PlainObjectBase): + pass + + cdef cppclass ArrayX2d(PlainObjectBase): + pass + + cdef cppclass ArrayX3d(PlainObjectBase): + pass + + cdef cppclass ArrayX4d(PlainObjectBase): + pass + + cdef cppclass ArrayXXd(PlainObjectBase): + pass + + cdef cppclass Array2d(PlainObjectBase): + pass + + cdef cppclass Array3d(PlainObjectBase): + pass + + cdef cppclass Array4d(PlainObjectBase): + pass + + cdef cppclass ArrayXd(PlainObjectBase): + pass + + cdef cppclass Array22cf(PlainObjectBase): + pass + + cdef cppclass Array23cf(PlainObjectBase): + pass + + cdef cppclass Array24cf(PlainObjectBase): + pass + + cdef cppclass Array2Xcf(PlainObjectBase): + pass + + cdef cppclass Array32cf(PlainObjectBase): + pass + + cdef cppclass Array33cf(PlainObjectBase): + pass + + cdef cppclass Array34cf(PlainObjectBase): + pass + + cdef cppclass Array3Xcf(PlainObjectBase): + pass + + cdef cppclass Array42cf(PlainObjectBase): + pass + + cdef cppclass Array43cf(PlainObjectBase): + pass + + cdef cppclass Array44cf(PlainObjectBase): + pass + + cdef cppclass Array4Xcf(PlainObjectBase): + pass + + cdef cppclass ArrayX2cf(PlainObjectBase): + pass + + cdef cppclass ArrayX3cf(PlainObjectBase): + pass + + cdef cppclass ArrayX4cf(PlainObjectBase): + pass + + cdef cppclass ArrayXXcf(PlainObjectBase): + pass + + cdef cppclass Array2cf(PlainObjectBase): + pass + + cdef cppclass Array3cf(PlainObjectBase): + pass + + cdef cppclass Array4cf(PlainObjectBase): + pass + + cdef cppclass ArrayXcf(PlainObjectBase): + pass + + cdef cppclass Array22cd(PlainObjectBase): + pass + + cdef cppclass Array23cd(PlainObjectBase): + pass + + cdef cppclass Array24cd(PlainObjectBase): + pass + + cdef cppclass Array2Xcd(PlainObjectBase): + pass + + cdef cppclass Array32cd(PlainObjectBase): + pass + + cdef cppclass Array33cd(PlainObjectBase): + pass + + cdef cppclass Array34cd(PlainObjectBase): + pass + + cdef cppclass Array3Xcd(PlainObjectBase): + pass + + cdef cppclass Array42cd(PlainObjectBase): + pass + + cdef cppclass Array43cd(PlainObjectBase): + pass + + cdef cppclass Array44cd(PlainObjectBase): + pass + + cdef cppclass Array4Xcd(PlainObjectBase): + pass + + cdef cppclass ArrayX2cd(PlainObjectBase): + pass + + cdef cppclass ArrayX3cd(PlainObjectBase): + pass + + cdef cppclass ArrayX4cd(PlainObjectBase): + pass + + cdef cppclass ArrayXXcd(PlainObjectBase): + pass + + cdef cppclass Array2cd(PlainObjectBase): + pass + + cdef cppclass Array3cd(PlainObjectBase): + pass + + cdef cppclass Array4cd(PlainObjectBase): + pass + + cdef cppclass ArrayXcd(PlainObjectBase): + pass + + diff --git a/cython/gtsam_eigency/core.pyx b/cython/gtsam_eigency/core.pyx new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/cython/gtsam_eigency/core.pyx @@ -0,0 +1 @@ + diff --git a/cython/gtsam_eigency/eigency_cpp.h b/cython/gtsam_eigency/eigency_cpp.h new file mode 100644 index 000000000..923ca7c94 --- /dev/null +++ b/cython/gtsam_eigency/eigency_cpp.h @@ -0,0 +1,504 @@ +#include + +#include +#include +#include + +typedef ::std::complex< double > __pyx_t_double_complex; +typedef ::std::complex< float > __pyx_t_float_complex; + +#include "conversions_api.h" + +#ifndef EIGENCY_CPP +#define EIGENCY_CPP + +namespace eigency { + +template +inline PyArrayObject *_ndarray_view(Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0); +template +inline PyArrayObject *_ndarray_copy(const Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0); + +// Strides: +// Eigen and numpy differ in their way of dealing with strides. Eigen has the concept of outer and +// inner strides, which are dependent on whether the array/matrix is row-major of column-major: +// Inner stride: denotes the offset between succeeding elements in each row (row-major) or column (column-major). +// Outer stride: denotes the offset between succeeding rows (row-major) or succeeding columns (column-major). +// In contrast, numpy's stride is simply a measure of how fast each dimension should be incremented. +// Consequently, a switch in numpy storage order from row-major to column-major involves a switch +// in strides, while it does not affect the stride in Eigen. +template<> +inline PyArrayObject *_ndarray_view(double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) { + // Eigen row-major mode: row_stride=outer_stride, and col_stride=inner_stride + // If no stride is given, the row_stride is set to the number of columns. + return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + } else { + // Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride + // If no stride is given, the cow_stride is set to the number of rows. + return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); + } +} +template<> +inline PyArrayObject *_ndarray_copy(const double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view >(std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy >(const std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view >(std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy >(const std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + + +template +inline PyArrayObject *ndarray(Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +// If C++11 is available, check if m is an r-value reference, in +// which case a copy should always be made +#if __cplusplus >= 201103L +template +inline PyArrayObject *ndarray(Eigen::PlainObjectBase &&m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +#endif +template +inline PyArrayObject *ndarray(const Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +template +inline PyArrayObject *ndarray_view(Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +template +inline PyArrayObject *ndarray_view(const Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(const_cast(m.data()), m.rows(), m.cols(), m.IsRowMajor); +} +template +inline PyArrayObject *ndarray_copy(const Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} + +template +inline PyArrayObject *ndarray(Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray(const Eigen::Map &m) { + import_gtsam_eigency__conversions(); + // Since this is a map, we assume that ownership is correctly taken care + // of, and we avoid taking a copy + return _ndarray_view(const_cast(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray_view(Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray_view(const Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(const_cast(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray_copy(const Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} + + +template > +class MapBase: public Eigen::Map { +public: + typedef Eigen::Map Base; + typedef typename Base::Scalar Scalar; + + MapBase(Scalar* data, + long rows, + long cols, + _StrideType stride=_StrideType()) + : Base(data, + // If both dimensions are dynamic or dimensions match, accept dimensions as they are + ((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) || + (Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols)) + ? rows + // otherwise, test if swapping them makes them fit + : ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows) + ? cols + : rows), + ((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) || + (Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols)) + ? cols + : ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows) + ? rows + : cols), + stride + ) {} + + MapBase &operator=(const MatrixType &other) { + Base::operator=(other); + return *this; + } + + virtual ~MapBase() { } +}; + + +template class EigencyDenseBase, + typename Scalar, + int _Rows, int _Cols, + int _Options = Eigen::AutoAlign | +#if defined(__GNUC__) && __GNUC__==3 && __GNUC_MINOR__==4 + // workaround a bug in at least gcc 3.4.6 + // the innermost ?: ternary operator is misparsed. We write it slightly + // differently and this makes gcc 3.4.6 happy, but it's ugly. + // The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined + // (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor) + ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor +// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19 +#if EIGEN_VERSION_AT_LEAST(3,2,90) + : !(_Cols==1 && _Rows!=1) ? EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION +#else + : !(_Cols==1 && _Rows!=1) ? Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION +#endif + : ColMajor ), +#else + ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor + : (_Cols==1 && _Rows!=1) ? Eigen::ColMajor +// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19 +#if EIGEN_VERSION_AT_LEAST(3,2,90) + : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ), +#else + : Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ), +#endif +#endif + int _MapOptions = Eigen::Unaligned, + int _StrideOuter=0, int _StrideInner=0, + int _MaxRows = _Rows, + int _MaxCols = _Cols> +class FlattenedMap: public MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > { +public: + typedef MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base; + + FlattenedMap() + : Base(NULL, 0, 0), + object_(NULL) {} + + FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0) + : Base(data, rows, cols, + Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)), + object_(NULL) { + } + + FlattenedMap(PyArrayObject *object) + : Base((Scalar *)((PyArrayObject*)object)->data, + // : Base(_from_numpy((PyArrayObject*)object), + (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, + (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0], + Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, + _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])), + object_(object) { + + if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object)) + throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); + + Py_XINCREF(object_); + } + FlattenedMap &operator=(const FlattenedMap &other) { + if (other.object_) { + new (this) FlattenedMap(other.object_); + } else { + // Replace the memory that we point to (not a memory allocation) + new (this) FlattenedMap(const_cast(other.data()), + other.rows(), + other.cols(), + other.outerStride(), + other.innerStride()); + } + + return *this; + } + + operator Base() const { + return static_cast(*this); + } + + operator Base&() const { + return static_cast(*this); + } + + operator EigencyDenseBase() const { + return EigencyDenseBase(static_cast(*this)); + } + + virtual ~FlattenedMap() { + Py_XDECREF(object_); + } + +private: + PyArrayObject * const object_; +}; + + +template +class Map: public MapBase { +public: + typedef MapBase Base; + typedef typename MatrixType::Scalar Scalar; + + enum { + RowsAtCompileTime = Base::Base::RowsAtCompileTime, + ColsAtCompileTime = Base::Base::ColsAtCompileTime + }; + + Map() + : Base(NULL, + (RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime, + (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime), + object_(NULL) { + } + + Map(Scalar *data, long rows, long cols) + : Base(data, rows, cols), + object_(NULL) {} + + Map(PyArrayObject *object) + : Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data, + // ROW: If array is in row-major order, transpose (see README) + (PyObject*)object == Py_None? 0 : + (!PyArray_IS_F_CONTIGUOUS(object) + ? ((object->nd == 1) + ? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector) + : object->dimensions[1]) + : object->dimensions[0]), + // COLUMN: If array is in row-major order: transpose (see README) + (PyObject*)object == Py_None? 0 : + (!PyArray_IS_F_CONTIGUOUS(object) + ? object->dimensions[0] + : ((object->nd == 1) + ? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector) + : object->dimensions[1]))), + object_(object) { + + if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object)) + throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); + Py_XINCREF(object_); + } + + Map &operator=(const Map &other) { + if (other.object_) { + new (this) Map(other.object_); + } else { + // Replace the memory that we point to (not a memory allocation) + new (this) Map(const_cast(other.data()), + other.rows(), + other.cols()); + } + + return *this; + } + + Map &operator=(const MatrixType &other) { + MapBase::operator=(other); + return *this; + } + + operator Base() const { + return static_cast(*this); + } + + operator Base&() const { + return static_cast(*this); + } + + operator MatrixType() const { + return MatrixType(static_cast(*this)); + } + + virtual ~Map() { + Py_XDECREF(object_); + } + +private: + PyArrayObject * const object_; +}; + + +} + +#endif + + + diff --git a/cython/requirements.txt b/cython/requirements.txt new file mode 100644 index 000000000..cd77b097d --- /dev/null +++ b/cython/requirements.txt @@ -0,0 +1,3 @@ +Cython>=0.25.2 +backports_abc>=0.5 +numpy>=1.12.0 diff --git a/doc/Code/LocalizationExample2.cpp b/doc/Code/LocalizationExample2.cpp new file mode 100644 index 000000000..7fce69566 --- /dev/null +++ b/doc/Code/LocalizationExample2.cpp @@ -0,0 +1,7 @@ +// add unary measurement factors, like GPS, on all three poses +noiseModel::Diagonal::shared_ptr unaryNoise = + noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y +graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); +graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); +graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp new file mode 100644 index 000000000..8b80f2b2a --- /dev/null +++ b/doc/Code/LocalizationFactor.cpp @@ -0,0 +1,14 @@ +class UnaryFactor: public NoiseModelFactor1 { + double mx_, my_; ///< X and Y measurements + +public: + UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): + NoiseModelFactor1(model, j), mx_(x), my_(y) {} + + Vector evaluateError(const Pose2& q, + boost::optional H = boost::none) const + { + 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(); + } +}; diff --git a/doc/Code/LocalizationOutput5.txt b/doc/Code/LocalizationOutput5.txt new file mode 100644 index 000000000..d04162f20 --- /dev/null +++ b/doc/Code/LocalizationOutput5.txt @@ -0,0 +1,18 @@ +Final Result: +Values with 3 values: +Value 1: (-1.5e-14, 1.3e-15, -1.4e-16) +Value 2: (2, 3.1e-16, -8.5e-17) +Value 3: (4, -6e-16, -8.2e-17) + +x1 covariance: + 0.0083 4.3e-19 -1.1e-18 + 4.3e-19 0.0094 -0.0031 + -1.1e-18 -0.0031 0.0082 +x2 covariance: + 0.0071 2.5e-19 -3.4e-19 + 2.5e-19 0.0078 -0.0011 + -3.4e-19 -0.0011 0.0082 +x3 covariance: + 0.0083 4.4e-19 1.2e-18 + 4.4e-19 0.0094 0.0031 + 1.2e-18 0.0031 0.018 diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp new file mode 100644 index 000000000..ef880384c --- /dev/null +++ b/doc/Code/OdometryExample.cpp @@ -0,0 +1,15 @@ +// Create an empty nonlinear factor graph +NonlinearFactorGraph graph; + +// Add a Gaussian prior on pose x_1 +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)); + +// Add two odometry factors +Pose2 odometry(2.0, 0.0, 0.0); +noiseModel::Diagonal::shared_ptr odometryNoise = + noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); +graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/doc/Code/OdometryMarginals.cpp b/doc/Code/OdometryMarginals.cpp new file mode 100644 index 000000000..e1b4ed411 --- /dev/null +++ b/doc/Code/OdometryMarginals.cpp @@ -0,0 +1,6 @@ +// Query the marginals +cout.precision(2); +Marginals marginals(graph, result); +cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; +cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; +cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; diff --git a/doc/Code/OdometryOptimize.cpp b/doc/Code/OdometryOptimize.cpp new file mode 100644 index 000000000..ee3c918d4 --- /dev/null +++ b/doc/Code/OdometryOptimize.cpp @@ -0,0 +1,9 @@ +// create (deliberatly inaccurate) initial estimate +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)); + +// optimize using Levenberg-Marquardt optimization +Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + diff --git a/doc/Code/OdometryOutput1.txt b/doc/Code/OdometryOutput1.txt new file mode 100644 index 000000000..cc34e8ef2 --- /dev/null +++ b/doc/Code/OdometryOutput1.txt @@ -0,0 +1,11 @@ +Factor Graph: +size: 3 +factor 0: PriorFactor on 1 + prior mean: (0, 0, 0) + noise model: diagonal sigmas [0.3; 0.3; 0.1]; +factor 1: BetweenFactor(1,2) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 2: BetweenFactor(2,3) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; diff --git a/doc/Code/OdometryOutput2.txt b/doc/Code/OdometryOutput2.txt new file mode 100644 index 000000000..acfa0b95d --- /dev/null +++ b/doc/Code/OdometryOutput2.txt @@ -0,0 +1,11 @@ +Initial Estimate: +Values with 3 values: +Value 1: (0.5, 0, 0.2) +Value 2: (2.3, 0.1, -0.2) +Value 3: (4.1, 0.1, 0.1) + +Final Result: +Values with 3 values: +Value 1: (-1.8e-16, 8.7e-18, -9.1e-19) +Value 2: (2, 7.4e-18, -2.5e-18) +Value 3: (4, -1.8e-18, -3.1e-18) diff --git a/doc/Code/OdometryOutput3.txt b/doc/Code/OdometryOutput3.txt new file mode 100644 index 000000000..e346ccb4d --- /dev/null +++ b/doc/Code/OdometryOutput3.txt @@ -0,0 +1,12 @@ +x1 covariance: + 0.09 1.1e-47 5.7e-33 + 1.1e-47 0.09 1.9e-17 + 5.7e-33 1.9e-17 0.01 +x2 covariance: + 0.13 4.7e-18 2.4e-18 + 4.7e-18 0.17 0.02 + 2.4e-18 0.02 0.02 +x3 covariance: + 0.17 2.7e-17 8.4e-18 + 2.7e-17 0.37 0.06 + 8.4e-18 0.06 0.03 diff --git a/doc/Code/PlanarSLAMExample.m b/doc/Code/PlanarSLAMExample.m new file mode 100644 index 000000000..8f63853e7 --- /dev/null +++ b/doc/Code/PlanarSLAMExample.m @@ -0,0 +1,25 @@ +% Create graph container and add factors to it +graph = NonlinearFactorGraph; + +% Create keys for variables +i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); +j1 = symbol('l',1); j2 = symbol('l',2); + +% Add prior +priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +% add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); + +% Add odometry +odometry = Pose2(2.0, 0.0, 0.0); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); + +% Add bearing/range measurement factors +degrees = pi/180; +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); diff --git a/doc/Code/PlanarSLAMExample.txt b/doc/Code/PlanarSLAMExample.txt new file mode 100644 index 000000000..507a6b5ec --- /dev/null +++ b/doc/Code/PlanarSLAMExample.txt @@ -0,0 +1,7 @@ +>> result +Values with 5 values: + l1: (2, 2) + l2: (4, 2) + x1: (-1.8e-16, 5.1e-17, -1.5e-17) + x2: (2, -5.8e-16, -4.6e-16) + x3: (4, -3.1e-15, -4.6e-16) diff --git a/doc/Code/Pose2SLAMExample-graph.m b/doc/Code/Pose2SLAMExample-graph.m new file mode 100644 index 000000000..0e384359c --- /dev/null +++ b/doc/Code/Pose2SLAMExample-graph.m @@ -0,0 +1,15 @@ +%% Initialize graph, initial estimate, and odometry noise +datafile = findExampleDataFile('w100.graph'); +model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); +[graph,initial] = load2D(datafile, model); + +%% Add a Gaussian prior on pose x_0 +priorMean = Pose2(0, 0, 0); +priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); +graph.add(PriorFactorPose2(0, priorMean, priorNoise)); + +%% Optimize using Levenberg-Marquardt optimization and get marginals +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely; +marginals = Marginals(graph, result); + diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp new file mode 100644 index 000000000..2e2b41704 --- /dev/null +++ b/doc/Code/Pose2SLAMExample.cpp @@ -0,0 +1,16 @@ +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)); + +// Add odometry factors +noiseModel::Diagonal::shared_ptr model = + noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); +graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); +graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); +graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); +graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + +// Add the loop closure constraint +graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + diff --git a/doc/Code/Pose2SLAMExample.m b/doc/Code/Pose2SLAMExample.m new file mode 100644 index 000000000..561a07b1e --- /dev/null +++ b/doc/Code/Pose2SLAMExample.m @@ -0,0 +1,14 @@ +graph = NonlinearFactorGraph; +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +graph.add(PriorFactorPose2(1, Pose2(0, 0, 0), priorNoise)); + +%% Add odometry factors +model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.add(BetweenFactorPose2(1, 2, Pose2(2, 0, 0 ), model)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2, 0, pi/2), model)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2, 0, pi/2), model)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2, 0, pi/2), model)); + +%% Add pose constraint +graph.add(BetweenFactorPose2(5, 2, Pose2(2, 0, pi/2), model)); + diff --git a/doc/Code/Pose3SLAMExample-graph.m b/doc/Code/Pose3SLAMExample-graph.m new file mode 100644 index 000000000..702928759 --- /dev/null +++ b/doc/Code/Pose3SLAMExample-graph.m @@ -0,0 +1,12 @@ +%% Initialize graph, initial estimate, and odometry noise +datafile = findExampleDataFile('sphere2500.txt'); +model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); +[graph,initial] = load3D(datafile, model, true, 2500); +plot3DTrajectory(initial, 'g-', false); % Plot Initial Estimate + +%% Read again, now with all constraints, and optimize +graph = load3D(datafile, model, false, 2500); +graph.add(NonlinearEqualityPose3(0, initial.atPose3(0))); +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely(); +plot3DTrajectory(result, 'r-', false); axis equal; diff --git a/doc/Code/SFMExample.m b/doc/Code/SFMExample.m new file mode 100644 index 000000000..80d7cf279 --- /dev/null +++ b/doc/Code/SFMExample.m @@ -0,0 +1,9 @@ +%% Add factors for all measurements +noise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); +for i = 1:length(Z), + for k = 1:length(Z{i}) + j = J{i}{k}; + G.add(GenericProjectionFactorCal3_S2( + Z{i}{k}, noise, symbol('x', i), symbol('p', j), K)); + end +end diff --git a/doc/Code/VisualISAMExample.cpp b/doc/Code/VisualISAMExample.cpp new file mode 100644 index 000000000..345913cd9 --- /dev/null +++ b/doc/Code/VisualISAMExample.cpp @@ -0,0 +1,24 @@ +int relinearizeInterval = 3; +NonlinearISAM isam(relinearizeInterval); + +// ... first frame initialization omitted ... + +// Loop over the different poses, adding the observations to iSAM +for (size_t i = 1; i < poses.size(); ++i) { + + // Add factors for each landmark observation + NonlinearFactorGraph graph; + for (size_t j = 0; j < points.size(); ++j) { + graph.add( + GenericProjectionFactor + (z[i][j], noise,Symbol('x', i), Symbol('l', j), K) + ); + } + + // Add an initial guess for the current pose + Values initialEstimate; + initialEstimate.insert(Symbol('x', i), initial_x[i]); + + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + } diff --git a/doc/Code/calls.txt b/doc/Code/calls.txt new file mode 100644 index 000000000..2afd2fd12 --- /dev/null +++ b/doc/Code/calls.txt @@ -0,0 +1,7 @@ +>> graph.error(initialEstimate) +ans = + 20.1086 + +>> graph.error(result) +ans = + 8.2631e-18 diff --git a/doc/Code/print.txt b/doc/Code/print.txt new file mode 100644 index 000000000..da8fb3b2a --- /dev/null +++ b/doc/Code/print.txt @@ -0,0 +1,23 @@ +>> priorNoise +diagonal sigmas [0.3; 0.3; 0.1]; + +>> graph +size: 6 +factor 0: PriorFactor on 1 + prior mean: (0, 0, 0) + noise model: diagonal sigmas [0.3; 0.3; 0.1]; +factor 1: BetweenFactor(1,2) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 2: BetweenFactor(2,3) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 3: BetweenFactor(3,4) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 4: BetweenFactor(4,5) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; +factor 5: BetweenFactor(5,2) + measured: (2, 0, 1.6) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; diff --git a/doc/Code/whos.txt b/doc/Code/whos.txt new file mode 100644 index 000000000..592fedca9 --- /dev/null +++ b/doc/Code/whos.txt @@ -0,0 +1,7 @@ +>> whos + Name Size Bytes Class + graph 1x1 112 gtsam.NonlinearFactorGraph + priorNoise 1x1 112 gtsam.noiseModel.Diagonal + model 1x1 112 gtsam.noiseModel.Diagonal + initialEstimate 1x1 112 gtsam.Values + optimizer 1x1 112 gtsam.LevenbergMarquardtOptimizer diff --git a/doc/common_macros.tex b/doc/common_macros.tex new file mode 100644 index 000000000..73648c6a4 --- /dev/null +++ b/doc/common_macros.tex @@ -0,0 +1,124 @@ +\global\long\def\Vector#1{{\bf #1}} + \global\long\def\Matrix#1{{\bf #1}} + + +\global\long\def\eq#1{equation (\ref{eq:=0000231})} + + +\global\long\def\eye#1{\Vector{I_{#1}}} + + +\global\long\def\leftsparrow#1{\stackrel{#1}{\leftarrow}} + \global\long\def\rightsparrow#1{\stackrel{#1}{\rightarrow}} + \global\long\def\chain{\mathcal{M}} + + +\global\long\def\define{\stackrel{\Delta}{=}} + + +\global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}} + + +\global\long\def\Norm#1{\Vert#1\Vert} + \global\long\def\SqrNorm#1{\Vert#1\Vert^{2}} + \global\long\def\Ltwo#1{\mathcal{L}^{2}\left(#1\right)} + + +\global\long\def\Normal#1#2#3{\mathcal{N}(#1;#2,#3)} + + +\global\long\def\LogNormal#1#2#3{ (#1-#2)^{T} #3^{-1} (#1-#2) } + + +\global\long\def\SqrMah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}^{2}} + + +\global\long\def\SqrZMah#1#2{\Vert{#1}\Vert_{#2}^{2}} + + +\global\long\def\Info#1#2#3{\mathcal{N}^{-1}(#1;#2,#3)} + + +\providecommand{\half}{\frac{1}{2}} + +\global\long\def\Mah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}} + \global\long\def\MahDeriv#1#2#3#4{\biggl(\deriv{#2}{#4}\biggr)^{T} #3^{-1} (#1-#2)} + + +\global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}} + \global\long\def\argmax#1{\mathop{\textrm{argmax \,}}_{#1}} + + + + +\global\long\def\deriv#1#2{\frac{\partial#1}{\partial#2}} + + +\global\long\def\at#1#2{#1\biggr\rvert_{#2}} + + +\global\long\def\Jac#1#2#3{ \at{\deriv{#1}{#2}} {#3} } + + + + +\global\long\def\Rone{\mathbb{R}} +\global\long\def\Pone{\mathbb{P}} + + +\global\long\def\Rtwo{\mathbb{R}^{2}} +\global\long\def\Ptwo{\mathbb{P}^{2}} + + +\global\long\def\Stwo{\mathbb{S}^{2}} + \global\long\def\Complex{\mathbb{C}} + + +\global\long\def\Z{\mathbb{Z}} + \global\long\def\Rplus{\mathbb{R}^{+}} + + +\global\long\def\SOtwo{SO(2)} +\global\long\def\sotwo{\mathfrak{so(2)}} +\global\long\def\skew#1{[#1]_{+}} + + +\global\long\def\SEtwo{SE(2)} +\global\long\def\setwo{\mathfrak{se(2)}} +\global\long\def\Skew#1{[#1]_{\times}} + + +\global\long\def\Rthree{\mathbb{R}^{3}} +\global\long\def\Pthree{\mathbb{P}^{3}} + + +\global\long\def\SOthree{SO(3)} +\global\long\def\sothree{\mathfrak{so(3)}} + + +\global\long\def\Rsix{\mathbb{R}^{6}} +\global\long\def\SEthree{SE(3)} +\global\long\def\sethree{\mathfrak{se(3)}} + + +\global\long\def\Rn{\mathbb{R}^{n}} + + +\global\long\def\Afftwo{Aff(2)} +\global\long\def\afftwo{\mathfrak{aff(2)}} + + +\global\long\def\SLthree{SL(3)} +\global\long\def\slthree{\mathfrak{sl(3)}} + + + + +\global\long\def\stirling#1#2{\genfrac{\{}{\}}{0pt}{}{#1}{#2}} + + +\global\long\def\matlabscript#1#2{\begin{itemize}\item[]\lstinputlisting[caption=#2,label=#1]{#1.m}\end{itemize}} + + +\global\long\def\atan{\mathop{atan2}} + diff --git a/doc/gtsam.bib b/doc/gtsam.bib new file mode 100644 index 000000000..8cb4f1a55 --- /dev/null +++ b/doc/gtsam.bib @@ -0,0 +1,133 @@ +@String { ICCV = {Intl. Conf. on Computer Vision (ICCV)} } +@String { IROS = {IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)} } +@String { CVPR = {IEEE Conf. on Computer Vision and Pattern Recognition (CVPR)} } +@String { IJRR = {Intl. J. of Robotics Research} } +@String { RAS = {Robotics and Autonomous Systems} } +@String { TRO = {{IEEE} Trans. Robotics} } +@String { IT = {{IEEE} Trans. Inform. Theory} } +@String { ISRR = {Proc. of the Intl. Symp. of Robotics Research (ISRR)} } + +@inproceedings{Davison03iccv, + title = {Real-Time Simultaneous Localisation and Mapping with a Single Camera}, + author = {A.J. Davison}, + booktitle = ICCV, + year = {2003}, + month = {Oct}, + pages = {1403-1410} +} + +@inproceedings{Dellaert10iros, + title = {Subgraph-preconditioned Conjugate Gradient for Large Scale SLAM}, + author = {F. Dellaert and J. Carlson and V. Ila and K. Ni and C.E. Thorpe}, + booktitle = IROS, + year = {2010}, +} + +@inproceedings{Dellaert99b, + title = {Using the Condensation Algorithm for Robust, Vision-based Mobile Robot Localization}, + author = {F. Dellaert and D. Fox and W. Burgard and S. Thrun}, + booktitle = CVPR, + year = {1999} +} + +@article{Dellaert06ijrr, + title = {Square {Root} {SAM}: Simultaneous Localization and Mapping via Square Root Information Smoothing}, + author = {F. Dellaert and M. Kaess}, + journal = IJRR, + year = {2006}, + month = {Dec}, + number = {12}, + pages = {1181--1203}, + volume = {25}, +} + +@article{DurrantWhyte06ram, + title = {Simultaneous Localisation and Mapping ({SLAM}): Part {I} The Essential Algorithms}, + author = {H.F. Durrant-Whyte and T. Bailey}, + journal = {Robotics \& Automation Magazine}, + year = {2006}, + month = {Jun}, +} + +@inproceedings{Jian11iccv, + title = {Generalized Subgraph Preconditioners for Large-Scale Bundle Adjustment}, + author = {Y.-D. Jian and D. Balcan and F. Dellaert}, + booktitle = ICCV, + year = {2011}, +} + +@article{Kaess09ras, + title = {Covariance Recovery from a Square Root Information Matrix for Data Association}, + author = {M. Kaess and F. Dellaert}, + journal = RAS, + year = {2009}, +} + +@article{Kaess12ijrr, + title = {{iSAM2}: Incremental Smoothing and Mapping Using the {B}ayes Tree}, + author = {M. Kaess and H. Johannsson and R. Roberts and V. Ila and J. Leonard and F. Dellaert}, + journal = IJRR, + year = {2012}, + month = {Feb}, + pages = {217--236}, + volume = {31}, + issue = {2}, +} + +@article{Kaess08tro, + title = {{iSAM}: Incremental Smoothing and Mapping}, + author = {M. Kaess and A. Ranganathan and F. Dellaert}, + journal = TRO, + year = {2008}, + month = {Dec}, + number = {6}, + pages = {1365-1378}, + volume = {24}, +} + +@book{Koller09book, + title = {Probabilistic Graphical Models: Principles and Techniques}, + author = {D. Koller and N. Friedman}, + publisher = {The MIT Press}, + year = {2009} +} + +@Article{Kschischang01it, + title = {Factor Graphs and the Sum-Product Algorithm}, + Author = {F.R. Kschischang and B.J. Frey and H-A. Loeliger}, + Journal = IT, + Year = {2001}, + + Month = {February}, + Number = {2}, + Volume = {47} +} + +@article{Loeliger04spm, + Title = {An Introduction to Factor Graphs}, + Author = {H.-A. Loeliger}, + Journal = {IEEE Signal Processing Magazine}, + Year = {2004}, + + Month = {January}, + Pages = {28--41} +} + +@inproceedings{Nister04cvpr2, + title = {Visual Odometry}, + author = {D. Nist\'er and O. Naroditsky and J. Bergen}, + booktitle = CVPR, + year = {2004}, + month = {Jun}, + pages = {652-659}, + volume = {1} +} + +@InProceedings{Smith87b, + title = {A stochastic map for uncertain spatial relationships}, + Author = {R. Smith and M. Self and P. Cheeseman}, + Booktitle = ISRR, + Year = {1988}, + Pages = {467-474} +} + diff --git a/doc/gtsam.lyx b/doc/gtsam.lyx new file mode 100644 index 000000000..29be8dbe4 --- /dev/null +++ b/doc/gtsam.lyx @@ -0,0 +1,3767 @@ +#LyX 2.1 created this file. For more info see http://www.lyx.org/ +\lyxformat 474 +\begin_document +\begin_header +\textclass article +\begin_preamble +\usepackage{times} +\usepackage{listings} + +\usepackage[noend]{algpseudocode} +\usepackage[usenames,dvipsnames]{color} +% This is the color used for MATLAB comments below +\definecolor{MyDarkGreen}{rgb}{0.0,0.4,0.0} + +% For faster processing, load Matlab syntax for listings +\lstloadlanguages{Matlab}% +\lstset{language=Matlab, % Use MATLAB + frame=single, % Single frame around code + basicstyle=\small\ttfamily, % Use small true type font + keywordstyle=[1]\color{Blue}\bf, % MATLAB functions bold and blue + keywordstyle=[2]\color{Purple}, % MATLAB function arguments purple + keywordstyle=[3]\color{Blue}\underbar, % User functions underlined and blue + identifierstyle=, % Nothing special about identifiers + % Comments small dark green courier + commentstyle=\usefont{T1}{pcr}{m}{sl}\color{MyDarkGreen}, + stringstyle=\color{Purple}, % Strings are purple + showstringspaces=false, % Don't put marks in string spaces + tabsize=5, % 5 spaces per tab + % + %%% Put standard MATLAB functions not included in the default + %%% language here + morekeywords={normpdf,normcdf,Pose2,Pose2SLAM}, + % + %%% Put MATLAB function parameters here + morekeywords=[2]{on, off, interp}, + % + %%% Put user defined functions here + morekeywords=[3]{FindESS, homework_example, gtsamSharedNoiseModel_Sigmas}, + % + morecomment=[l][\color{Blue}]{...}, % Line continuation (...) like blue comment + numbers=left, % Line numbers on left + firstnumber=1, % Line numbers start with line 1 + numberstyle=\tiny\color{Blue}, % Line numbers are blue + stepnumber=1 % Line numbers go in steps of 1 + } +\end_preamble +\use_default_options false +\maintain_unincluded_children false +\language english +\language_package default +\inputencoding auto +\fontencoding T1 +\font_roman ae +\font_sans default +\font_typewriter default +\font_math auto +\font_default_family rmdefault +\use_non_tex_fonts false +\font_sc false +\font_osf false +\font_sf_scale 100 +\font_tt_scale 100 +\graphics default +\default_output_format default +\output_sync 0 +\bibtex_command default +\index_command default +\paperfontsize 10 +\spacing onehalf +\use_hyperref false +\papersize custom +\use_geometry true +\use_package amsmath 1 +\use_package amssymb 1 +\use_package cancel 0 +\use_package esint 0 +\use_package mathdots 1 +\use_package mathtools 0 +\use_package mhchem 1 +\use_package stackrel 0 +\use_package stmaryrd 0 +\use_package undertilde 0 +\cite_engine natbib +\cite_engine_type authoryear +\biblio_style plainnat +\use_bibtopic false +\use_indices false +\paperorientation portrait +\suppress_date false +\justification true +\use_refstyle 0 +\index Index +\shortcut idx +\color #008000 +\end_index +\paperwidth 7.44in +\paperheight 9.68in +\leftmargin 1in +\topmargin 1in +\rightmargin 1in +\bottommargin 1in +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\paragraph_indentation default +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false +\end_header + +\begin_body + +\begin_layout Title +Factor Graphs and GTSAM: +\begin_inset Newline newline +\end_inset + +A Hands-on Introduction +\end_layout + +\begin_layout Author +Frank Dellaert +\begin_inset Newline newline +\end_inset + +Technical Report number GT-RIM-CP&R-2014-XXX +\end_layout + +\begin_layout Date +September 2014 +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand input +filename "common_macros.tex" + +\end_inset + + +\end_layout + +\begin_layout Section* +Overview +\end_layout + +\begin_layout Standard +In this document I provide a hands-on introduction to both factor graphs + and GTSAM. + This is an updated version from the 2012 TR that is tailored to our GTSAM + 3.0 library and beyond. +\end_layout + +\begin_layout Standard + +\series bold +Factor graphs +\series default + are graphical models +\begin_inset CommandInset citation +LatexCommand citep +key "Koller09book" + +\end_inset + + that are well suited to modeling complex estimation problems, such as Simultane +ous Localization and Mapping (SLAM) or Structure from Motion (SFM). + You might be familiar with another often used graphical model, Bayes networks, + which are directed acyclic graphs. + A +\series bold +factor graph, +\series default +however, is a +\emph on +bipartite +\emph default + graph consisting of factors connected to variables. + The +\series bold +variables +\series default + represent the unknown random variables in the estimation problem, whereas + the +\series bold +factors +\series default + represent probabilistic constraints on those variables, derived from measuremen +ts or prior knowledge. + In the following sections I will illustrate this with examples from both + robotics and vision. +\end_layout + +\begin_layout Standard +The GTSAM toolbox (GTSAM stands for +\begin_inset Quotes eld +\end_inset + +Georgia Tech Smoothing and Mapping +\begin_inset Quotes erd +\end_inset + +) toolbox is a BSD-licensed C++ library based on factor graphs, developed + at the Georgia Institute of Technology by myself, many of my students, + and collaborators. + It provides state of the art solutions to the SLAM and SFM problems, but + can also be used to model and solve both simpler and more complex estimation + problems. + It also provides a MATLAB interface which allows for rapid prototype developmen +t, visualization, and user interaction. +\end_layout + +\begin_layout Standard +GTSAM exploits sparsity to be computationally efficient. + Typically measurements only provide information on the relationship between + a handful of variables, and hence the resulting factor graph will be sparsely + connected. + This is exploited by the algorithms implemented in GTSAM to reduce computationa +l complexity. + Even when graphs are too dense to be handled efficiently by direct methods, + GTSAM provides iterative methods that are quite efficient regardless. +\end_layout + +\begin_layout Standard +You can download the latest version of GTSAM at +\begin_inset Flex URL +status open + +\begin_layout Plain Layout + +http://tinyurl.com/gtsam +\end_layout + +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset CommandInset toc +LatexCommand tableofcontents + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +Factor Graphs +\end_layout + +\begin_layout Standard +Let us start with a one-page primer on factor graphs, which in no way replaces + the excellent and detailed reviews by +\begin_inset CommandInset citation +LatexCommand citet +key "Kschischang01it" + +\end_inset + + and +\begin_inset CommandInset citation +LatexCommand citet +key "Loeliger04spm" + +\end_inset + +. + +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/hmm.pdf + scale 60 + BoundingBox 40bp 37bp 400bp 150bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:unrolledHMM" + +\end_inset + +An HMM, unrolled over three time-steps, represented by a Bayes net. + +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:unrolledHMM" + +\end_inset + + shows the +\series bold +Bayes network +\series default + for a hidden Markov model (HMM) over three time steps. + In a Bayes net, each node is associated with a conditional density: the + top Markov chain encodes the prior +\begin_inset Formula $P(X_{1})$ +\end_inset + + and transition probabilities +\begin_inset Formula $P(X_{2}|X_{1})$ +\end_inset + + and +\begin_inset Formula $P(X_{3}|X_{2})$ +\end_inset + +, whereas measurements +\begin_inset Formula $Z_{t}$ +\end_inset + + depend only on the state +\begin_inset Formula $X_{t}$ +\end_inset + +, modeled by conditional densities +\begin_inset Formula $P(Z_{t}|X_{t})$ +\end_inset + +. + Given known measurements +\begin_inset Formula $z_{1}$ +\end_inset + +, +\begin_inset Formula $z_{2}$ +\end_inset + + and +\begin_inset Formula $z_{3}$ +\end_inset + + we are interested in the hidden state sequence +\begin_inset Formula $(X_{1},X_{2},X_{3})$ +\end_inset + + that maximizes the posterior probability +\begin_inset Formula $P(X_{1},X_{2},X_{3}|Z_{1}=z_{1},Z_{2}=z_{2},Z_{3}=z_{3})$ +\end_inset + +. + Since the measurements +\begin_inset Formula $Z_{1}$ +\end_inset + +, +\begin_inset Formula $Z_{2}$ +\end_inset + +, and +\begin_inset Formula $Z_{3}$ +\end_inset + + are +\emph on +known +\emph default +, the posterior is proportional to the product of six +\series bold +factors +\series default +, three of which derive from the the Markov chain, and three likelihood + factors defined as +\begin_inset Formula $L(X_{t};z)\propto P(Z_{t}=z|X_{t})$ +\end_inset + +: +\begin_inset Formula +\[ +P(X_{1},X_{2},X_{3}|Z_{1},Z_{2},Z_{3})\propto P(X_{1})P(X_{2}|X_{1})P(X_{3}|X_{2})L(X_{1};z_{1})L(X_{2};z_{2})L(X_{3};z_{3}) +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +vspace{-3mm} +\end_layout + +\end_inset + + +\begin_inset Float figure +placement H +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/hmm-FG.pdf + scale 60 + BoundingBox 30bp 40bp 340bp 130bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:HMM-FG" + +\end_inset + +An HMM with observed measurements, unrolled over time, represented as a + factor graph. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + This motivates a different graphical model, a +\series bold +factor graph +\series default +, in which we only represent the unknown variables +\begin_inset Formula $X_{1}$ +\end_inset + +, +\begin_inset Formula $X_{2}$ +\end_inset + +, and +\begin_inset Formula $X_{3}$ +\end_inset + +, connected to factors that encode probabilistic information on them, as + in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:HMM-FG" + +\end_inset + +. + To do maximum a-posteriori (MAP) inference, we then maximize the product + +\begin_inset Formula +\[ +f(X_{1},X_{2},X_{3})=\prod f_{i}(\mathcal{X}_{i}) +\] + +\end_inset + +i.e., the value of the factor graph. + It should be clear from the figure that the connectivity of a factor graph + encodes, for each factor +\begin_inset Formula $f_{i}$ +\end_inset + +, which subset of variables +\begin_inset Formula $\mathcal{X}_{i}$ +\end_inset + + it depends on. + In the examples below, we use factor graphs to model more complex MAP inference + problems in robotics. +\end_layout + +\begin_layout Section +\begin_inset CommandInset label +LatexCommand label +name "sec:Robot-Localization" + +\end_inset + +Modeling Robot Motion +\end_layout + +\begin_layout Subsection +Modeling with Factor Graphs +\end_layout + +\begin_layout Standard +Before diving into a SLAM example, let us consider the simpler problem of + modeling robot motion. + This can be done with a +\emph on +continuous +\emph default + Markov chain, and provides a gentle introduction to GTSAM. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/FactorGraph.pdf + scale 80 + BoundingBox 40bp 585bp 300bp 625bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:OdometryFG" + +\end_inset + +Factor graph for robot localization. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + The factor graph for a simple example is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:OdometryFG" + +\end_inset + +. + There are three variables +\begin_inset Formula $x_{1}$ +\end_inset + +, +\begin_inset Formula $x_{2}$ +\end_inset + +, and +\begin_inset Formula $x_{3}$ +\end_inset + + which represent the poses of the robot over time, rendered in the figure + by the open-circle variable nodes. + In this example, we have one +\series bold +unary factor +\series default + +\begin_inset Formula $f_{0}(x_{1})$ +\end_inset + + on the first pose +\begin_inset Formula $x_{1}$ +\end_inset + + that encodes our prior knowledge about +\begin_inset Formula $x_{1}$ +\end_inset + +, and two +\series bold +binary factors +\series default + that relate successive poses, respectively +\begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$ +\end_inset + + and +\begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$ +\end_inset + +, where +\begin_inset Formula $o_{1}$ +\end_inset + + and +\begin_inset Formula $o_{2}$ +\end_inset + + represent odometry measurements. +\end_layout + +\begin_layout Subsection +Creating a Factor Graph +\end_layout + +\begin_layout Standard +The following C++ code, included in GTSAM as an example, creates the factor + graph in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:OdometryFG" + +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/OdometryExample.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/OdometryExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:OdometryExample},language={C++},numbers=left" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + Above, line 2 creates an empty factor graph. + We then add the factor +\begin_inset Formula $f_{0}(x_{1})$ +\end_inset + + on lines 5-8 as an instance of +\series bold +\emph on +PriorFactor +\series default +\emph default +, a templated class provided in the slam subfolder, with +\series bold +\emph on +T=Pose2 +\series default +\emph default +. + Its constructor takes a variable +\series bold +\emph on +Key +\series default +\emph default + (in this case 1), a mean of type +\series bold +\emph on +Pose2, +\series default +\emph default + created on Line 5, and a noise model for the prior density. + We provide a diagonal Gaussian of type +\series bold +\emph on +noiseModel::Diagonal +\series default +\emph default + by specifying three standard deviations in line 7, respectively 30 cm. +\begin_inset space ~ +\end_inset + +on the robot's position, and 0.1 radians on the robot's orientation. + Note that the +\series bold +\emph on +Sigmas +\series default +\emph default + constructor returns a shared pointer, anticipating that typically the same + noise models are used for many different factors. +\end_layout + +\begin_layout Standard +Similarly, odometry measurements are specified as +\series bold +\emph on +Pose2 +\series default +\emph default + on line 11, with a slightly different noise model defined on line 12-13. + We then add the two factors +\begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$ +\end_inset + + and +\begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$ +\end_inset + + on lines 14-15, as instances of yet another templated class, +\series bold +\emph on +BetweenFactor +\series default +\emph default +, again with +\series bold +\emph on +T=Pose2 +\series default +\emph default +. +\end_layout + +\begin_layout Standard +When running the example ( +\emph on +make OdometryExample.run +\emph default + on the command prompt), it will print out the factor graph as follows: +\family typewriter +\size small + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/OdometryOutput1.txt" + +\end_inset + + +\end_layout + +\begin_layout Subsection +Factor Graphs versus Values +\end_layout + +\begin_layout Standard +At this point it is instructive to emphasize two important design ideas + underlying GTSAM: +\end_layout + +\begin_layout Enumerate +The factor graph and its embodiment in code specify the joint probability + distribution +\begin_inset Formula $P(X|Z)$ +\end_inset + + over the +\emph on +entire +\emph default + trajectory +\begin_inset Formula $X\define\{x_{1},x_{2},x_{3}\}$ +\end_inset + + of the robot, rather than just the last pose. + This +\emph on +smoothing +\emph default + view of the world gives GTSAM its name: +\begin_inset Quotes eld +\end_inset + +smoothing and mapping +\begin_inset Quotes erd +\end_inset + +. + Later in this document we will talk about how we can also use GTSAM to + do filtering (which you often do +\emph on +not +\emph default + want to do) or incremental inference (which we do all the time). +\end_layout + +\begin_layout Enumerate +A factor graph in GTSAM is just the specification of the probability density + +\begin_inset Formula $P(X|Z)$ +\end_inset + +, and the corresponding +\series bold +\emph on +FactorGraph +\series default +\emph default + class and its derived classes do not ever contain a +\begin_inset Quotes eld +\end_inset + +solution +\begin_inset Quotes erd +\end_inset + +. + Rather, there is a separate type +\series bold +\emph on +Values +\series default +\emph default + that is used to specify specific values for (in this case) +\begin_inset Formula $x_{1}$ +\end_inset + +, +\begin_inset Formula $x_{2}$ +\end_inset + +, and +\begin_inset Formula $x_{3}$ +\end_inset + +, which can then be used to evaluate the probability (or, more commonly, + the error) associated with particular values. +\end_layout + +\begin_layout Standard +The latter point is often a point of confusion with beginning users of GTSAM. + It helps to remember that when designing GTSAM we took a functional approach + of classes corresponding to mathematical objects, which are usually immutable. + You should think of a factor graph as a +\emph on +function +\emph default + to be applied to values -as the notation +\begin_inset Formula $f(X)\propto P(X|Z)$ +\end_inset + + implies- rather than as an object to be modified. +\end_layout + +\begin_layout Subsection +Non-linear Optimization in GTSAM +\end_layout + +\begin_layout Standard +The listing below creates a +\series bold +\emph on +Values +\series default +\emph default + instance, and uses it as the initial estimate to find the maximum a-posteriori + (MAP) assignment for the trajectory +\begin_inset Formula $X$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/OdometryOptimize.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/OdometryExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:OdometryOptimize},language={C++},numbers=left" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + Lines 2-5 in Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:OdometryOptimize" + +\end_inset + + create the initial estimate, and on line 8 we create a non-linear Levenberg-Mar +quardt style optimizer, and call +\series bold +\emph on +optimize +\series default +\emph default + using default parameter settings. + The reason why GTSAM needs to perform non-linear optimization is because + the odometry factors +\begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$ +\end_inset + + and +\begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$ +\end_inset + + are non-linear, as they involve the orientation of the robot. + This also explains why the factor graph we created in Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:OdometryExample" + +\end_inset + + is of type +\series bold +\emph on +NonlinearFactorGraph +\series default +\emph default +. + The optimization class linearizes this graph, possibly multiple times, + to minimize the non-linear squared error specified by the factors. +\end_layout + +\begin_layout Standard +The relevant output from running the example is as follows: +\family typewriter +\size small + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/OdometryOutput2.txt" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + It can be seen that, subject to very small tolerance, the ground truth + solution +\begin_inset Formula $x_{1}=(0,0,0)$ +\end_inset + +, +\begin_inset Formula $x_{2}=(2,0,0)$ +\end_inset + +, and +\begin_inset Formula $x_{3}=(4,0,0)$ +\end_inset + + is recovered. +\end_layout + +\begin_layout Subsection +\begin_inset CommandInset label +LatexCommand label +name "sub:Full-Posterior-Inference" + +\end_inset + +Full Posterior Inference +\end_layout + +\begin_layout Standard +GTSAM can also be used to calculate the covariance matrix for each pose + after incorporating the information from all measurements +\begin_inset Formula $Z$ +\end_inset + +. + Recognizing that the factor graph encodes the +\series bold +posterior density +\series default + +\begin_inset Formula $P(X|Z)$ +\end_inset + +, the mean +\begin_inset Formula $\mu$ +\end_inset + + together with the covariance +\begin_inset Formula $\Sigma$ +\end_inset + + for each pose +\begin_inset Formula $x$ +\end_inset + + approximate the +\series bold +marginal posterior density +\series default + +\begin_inset Formula $P(x|Z)$ +\end_inset + +. + Note that this is just an approximation, as even in this simple case the + odometry factors are actually non-linear in their arguments, and GTSAM + only computes a Gaussian approximation to the true underlying posterior. +\end_layout + +\begin_layout Standard +The following C++ code will recover the posterior marginals: +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/OdometryMarginals.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/OdometryExample.cpp},label={listing:OdometryMarginals}" + +\end_inset + +The relevant output from running the example is as follows: +\size footnotesize + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/OdometryOutput3.txt" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + What we see is that the marginal covariance +\begin_inset Formula $P(x_{1}|Z)$ +\end_inset + + on +\begin_inset Formula $x_{1}$ +\end_inset + + is simply the prior knowledge on +\begin_inset Formula $x_{1}$ +\end_inset + +, but as the robot moves the uncertainty in all dimensions grows without + bound, and the +\begin_inset Formula $y$ +\end_inset + + and +\begin_inset Formula $\theta$ +\end_inset + + components of the pose become (positively) correlated. +\end_layout + +\begin_layout Standard +An important fact to note when interpreting these numbers is that covariance + matrices are given in +\emph on +relative +\emph default + coordinates, not absolute coordinates. + This is because internally GTSAM optimizes for a change with respect to + a linearization point, as do all nonlinear optimization libraries. +\end_layout + +\begin_layout Section +Robot Localization +\end_layout + +\begin_layout Subsection +Unary Measurement Factors +\end_layout + +\begin_layout Standard +In this section we add measurements to the factor graph that will help us + actually +\emph on +localize +\emph default + the robot over time. + The example also serves as a tutorial on creating new factor types. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/FactorGraph2.pdf + scale 80 + BoundingBox 70bp 550bp 300bp 630bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:LocalizationFG" + +\end_inset + +Robot localization factor graph with unary measurement factors at each time + step. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In particular, we use +\series bold +unary measurement factors +\series default + to handle external measurements. + The example from Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Robot-Localization" + +\end_inset + + is not very useful on a real robot, because it only contains factors correspond +ing to odometry measurements. + These are imperfect and will lead to quickly accumulating uncertainty on + the last robot pose, at least in the absence of any external measurements + (see Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sub:Full-Posterior-Inference" + +\end_inset + +). + Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:LocalizationFG" + +\end_inset + + shows a new factor graph where the prior +\begin_inset Formula $f_{0}(x_{1})$ +\end_inset + + is omitted and instead we added three unary factors +\begin_inset Formula $f_{1}(x_{1};z_{1})$ +\end_inset + +, +\begin_inset Formula $f_{2}(x_{2};z_{2})$ +\end_inset + +, and +\begin_inset Formula $f_{3}(x_{3};z_{3})$ +\end_inset + +, one for each localization measurement +\begin_inset Formula $z_{t}$ +\end_inset + +, respectively. + Such unary factors are applicable for measurements +\begin_inset Formula $z_{t}$ +\end_inset + + that depend +\emph on +only +\emph default + on the current robot pose, e.g., GPS readings, correlation of a laser range-finde +r in a pre-existing map, or indeed the presence of absence of ceiling lights + (see +\begin_inset CommandInset citation +LatexCommand citet +key "Dellaert99b" + +\end_inset + + for that amusing example). +\end_layout + +\begin_layout Subsection +Defining Custom Factors +\end_layout + +\begin_layout Standard +In GTSAM, you can create custom unary factors by deriving a new class from + the built-in class +\series bold +\emph on +NoiseModelFactor1 +\series default +\emph default +, which implements a unary factor corresponding to a measurement likelihood + with a Gaussian noise model, +\begin_inset Formula +\[ +L(q;m)=exp\left\{ -\frac{1}{2}\SqrMah{h(q)}{m}{\Sigma}\right\} \define f(q) +\] + +\end_inset + +where +\begin_inset Formula $m$ +\end_inset + + is the measurement, +\begin_inset Formula $q$ +\end_inset + + is the unknown variable, +\begin_inset Formula $h(q)$ +\end_inset + + is a (possibly nonlinear) measurement function, and +\begin_inset Formula $\Sigma$ +\end_inset + + is the noise covariance. + Note that +\begin_inset Formula $m$ +\end_inset + + is considered +\emph on +known +\emph default + above, and the likelihood +\begin_inset Formula $L(q;m)$ +\end_inset + + +\begin_inset Note Note +status open + +\begin_layout Plain Layout + of +\begin_inset Formula $q$ +\end_inset + + given +\begin_inset Formula $m$ +\end_inset + + +\end_layout + +\end_inset + + will only ever be evaluated as a function of +\begin_inset Formula $q$ +\end_inset + +, which explains why it is a unary factor +\begin_inset Formula $f(q)$ +\end_inset + +. + It is always the unknown variable +\begin_inset Formula $q$ +\end_inset + + that is either likely or unlikely, given the measurement. + +\end_layout + +\begin_layout Standard + +\series bold +Note: +\series default +many people get this backwards, often misled by the conditional density + notation +\begin_inset Formula $P(m|q)$ +\end_inset + +. + In fact, the likelihood +\begin_inset Formula $L(q;m)$ +\end_inset + + is +\emph on +defined +\emph default + as any function of +\begin_inset Formula $q$ +\end_inset + + proportional to +\begin_inset Formula $P(m|q)$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Listing +\begin_inset CommandInset ref +LatexCommand vref +reference "listing:LocalizationFactor" + +\end_inset + + shows an example on how to define the custom factor class +\series bold +\emph on +UnaryFactor +\series default +\emph default + which implements a +\begin_inset Quotes eld +\end_inset + +GPS-like +\begin_inset Quotes erd +\end_inset + + measurement likelihood: +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/LocalizationFactor.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationFactor},language={C++},numbers=left" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In defining the derived class on line 1, we provide the template argument + +\series bold +\emph on +Pose2 +\series default +\emph default + to indicate the type of the variable +\begin_inset Formula $q$ +\end_inset + +, whereas the measurement is stored as the instance variables +\series bold +\emph on +mx_ +\series default +\emph default + and +\series bold +\emph on +my_ +\series default +\emph default +, defined on line 2. + The constructor on lines 5-6 simply passes on the variable key +\begin_inset Formula $j$ +\end_inset + + and the noise model to the superclass, and stores the measurement values + provided. + The most important function to has be implemented by every factor class + is +\series bold +\emph on +evaluateError +\series default +\emph default +, which should return +\begin_inset Formula +\[ +E(q)\define h(q)-m +\] + +\end_inset + +which is done on line 12. + Importantly, because we want to use this factor for nonlinear optimization + (see e.g., +\begin_inset CommandInset citation +LatexCommand citealt +key "Dellaert06ijrr" + +\end_inset + + for details), whenever the optional argument +\begin_inset Formula $H$ +\end_inset + + is provided, a +\series bold +\emph on +Matrix +\series default +\emph default + reference, the function should assign the +\series bold +Jacobian +\series default + of +\begin_inset Formula $h(q)$ +\end_inset + + to it, evaluated at the provided value for +\begin_inset Formula $q$ +\end_inset + +. + This is done for this example on line 11. + In this case, the Jacobian of the 2-dimensional function +\begin_inset Formula $h$ +\end_inset + +, which just returns the position of the robot, +\begin_inset Formula +\[ +h(q)=\left[\begin{array}{c} +q_{x}\\ +q_{y} +\end{array}\right] +\] + +\end_inset + + with respect the 3-dimensional pose +\begin_inset Formula $q=\left(q_{x},q_{y},q_{\theta}\right)$ +\end_inset + +, yields the following simple +\begin_inset Formula $2\times3$ +\end_inset + + matrix: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +H=\left[\begin{array}{ccc} +1 & 0 & 0\\ +0 & 1 & 0 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Using Custom Factors +\end_layout + +\begin_layout Standard +The following C++ code fragment illustrates how to create and add custom + factors to a factor graph: +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/LocalizationExample2.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationExample2},language={C++},numbers=left" + +\end_inset + + +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In Listing +\begin_inset CommandInset ref +LatexCommand vref +reference "listing:LocalizationExample2" + +\end_inset + +, we create the noise model on line 2-3, which now specifies two standard + deviations on the measurements +\begin_inset Formula $m_{x}$ +\end_inset + + and +\begin_inset Formula $m_{y}$ +\end_inset + +. + On lines 4-6 we create +\series bold +\emph on +shared_ptr +\series default +\emph default + versions of three newly created +\series bold +\emph on +UnaryFactor +\series default +\emph default + instances, and add them to graph. + GTSAM uses shared pointers to refer to factors in factor graphs, and +\series bold +\emph on +boost::make_shared +\series default +\emph default + is a convenience function to simultaneously construct a class and create + a +\series bold +\emph on +shared_ptr +\series default +\emph default + to it. + +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +and on lines 4-6 we add three newly created +\series bold +\emph on +UnaryFactor +\series default +\emph default + instances to the graph. +\end_layout + +\end_inset + + We obtain the factor graph from Figure +\begin_inset CommandInset ref +LatexCommand vref +reference "fig:LocalizationFG" + +\end_inset + +. + +\family typewriter +\size small + +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +The relevant output from running the example is as follows: +\family typewriter +\size small + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/LocalizationOutput4.txt" + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Subsection +Full Posterior Inference +\end_layout + +\begin_layout Standard +The three GPS factors are enough to fully constrain all unknown poses and + tie them to a +\begin_inset Quotes eld +\end_inset + +global +\begin_inset Quotes erd +\end_inset + + reference frame, including the three unknown orientations. + If not, GTSAM would have exited with a singular matrix exception. + The marginals can be recovered exactly as in Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sub:Full-Posterior-Inference" + +\end_inset + +, and the solution and marginal covariances are now given by the following: +\size footnotesize + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/LocalizationOutput5.txt" + +\end_inset + + +\end_layout + +\begin_layout Standard +Comparing this with the covariance matrices in Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sub:Full-Posterior-Inference" + +\end_inset + +, we can see that the uncertainty no longer grows without bounds as measurement + uncertainty accumulates. + Instead, the +\begin_inset Quotes eld +\end_inset + +GPS +\begin_inset Quotes erd +\end_inset + + measurements more or less constrain the poses evenly, as expected. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Float figure +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/Odometry.pdf + width 80text% + BoundingBox 70bp 310bp 525bp 500bp + clip + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Caption Standard + +\begin_layout Plain Layout +Odometry marginals +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\align center +\begin_inset space ~ +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\align center +\begin_inset Float figure +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/Localization.pdf + width 80text% + BoundingBox 70bp 310bp 525bp 500bp + clip + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Caption Standard + +\begin_layout Plain Layout +Localization Marginals +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:CompareMarginals" + +\end_inset + +Comparing the marginals resulting from the +\begin_inset Quotes eld +\end_inset + +odometry +\begin_inset Quotes erd +\end_inset + + factor graph in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:OdometryFG" + +\end_inset + + and the +\begin_inset Quotes eld +\end_inset + +localization +\begin_inset Quotes erd +\end_inset + + factor graph in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:LocalizationFG" + +\end_inset + +. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +It helps a lot when we view this graphically, as in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:CompareMarginals" + +\end_inset + +, where I show the marginals on position as covariance ellipses that contain + 68.26% of all probability mass. + For the odometry marginals, it is immediately apparent from the figure + that (1) the uncertainty on pose keeps growing, and (2) the uncertainty + on angular odometry translates into increasing uncertainty on y. + The localization marginals, in contrast, are constrained by the unary factors + and are all much smaller. + In addition, while less apparent, the uncertainty on the middle pose is + actually smaller as it is constrained by odometry from two sides. +\end_layout + +\begin_layout Standard +You might now be wondering how we produced these figures. + The answer is via the MATLAB interface of GTSAM, which we will demonstrate + in the next section. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +\begin_inset CommandInset label +LatexCommand label +name "sec:Pose2SLAM" + +\end_inset + + +\begin_inset CommandInset label +LatexCommand label +name "sec:WithMarginals" + +\end_inset + +PoseSLAM +\end_layout + +\begin_layout Subsection +Loop Closure Constraints +\end_layout + +\begin_layout Standard +The simplest instantiation of a SLAM problem is +\series bold +PoseSLAM +\series default +, which avoids building an explicit map of the environment. + The goal of SLAM is to simultaneously localize a robot and map the environment + given incoming sensor measurements +\begin_inset CommandInset citation +LatexCommand citep +key "DurrantWhyte06ram" + +\end_inset + +. + Besides wheel odometry, one of the most popular sensors for robots moving + on a plane is a 2D laser-range finder, which provides both odometry constraints + between successive poses, and loop-closure constraints when the robot re-visits + a previously explored part of the environment. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/FactorGraph3.pdf + scale 80 + BoundingBox 40bp 585bp 330bp 710bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:Pose2SLAM" + +\end_inset + +Factor graph for PoseSLAM. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + A factor graph example for PoseSLAM is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:Pose2SLAM" + +\end_inset + +. + The following C++ code, included in GTSAM as an example, creates this factor + graph in code: +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/Pose2SLAMExample.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/Pose2SLAMExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:Pose2SLAMExample},language={C++},numbers=left" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + As before, lines 1-4 create a nonlinear factor graph and add the unary + factor +\begin_inset Formula $f_{0}(x_{1})$ +\end_inset + +. + As the robot travels through the world, it creates binary factors +\begin_inset Formula $f_{t}(x_{t},x_{t+1})$ +\end_inset + + corresponding to odometry, added to the graph in lines 6-12 (Note that + M_PI_2 refers to pi/2). + But line 15 models a different event: a +\series bold +loop closure +\series default +. + For example, the robot might recognize the same location using vision or + a laser range finder, and calculate the geometric pose constraint to when + it first visited this location. + This is illustrated for poses +\begin_inset Formula $x_{5}$ +\end_inset + + and +\begin_inset Formula $x_{2}$ +\end_inset + +, and generates the (red) loop closing factor +\begin_inset Formula $f_{5}(x_{5},x_{2})$ +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/example1.pdf + width 80text% + BoundingBox 30bp 170bp 610bp 630bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:example" + +\end_inset + +The result of running optimize on the factor graph in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:Pose2SLAM" + +\end_inset + +. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +We can optimize this factor graph as before, by creating an initial estimate + of type +\series bold +\emph on +Values +\series default +\emph default +, and creating and running an optimizer. + The result is shown graphically in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:example" + +\end_inset + +, along with covariance ellipses shown in green. + These covariance ellipses in 2D indicate the marginal over position, over + all possible orientations, and show the area which contain 68.26% of the + probability mass (in 1D this would correspond to one standard deviation). + The graph shows in a clear manner that the uncertainty on pose +\begin_inset Formula $x_{5}$ +\end_inset + + is now much less than if there would be only odometry measurements. + The pose with the highest uncertainty, +\begin_inset Formula $x_{4}$ +\end_inset + +, is the one furthest away from the unary constraint +\begin_inset Formula $f_{0}(x_{1})$ +\end_inset + +, which is the only factor tying the graph to a global coordinate frame. +\end_layout + +\begin_layout Standard +The figure above was created using an interface that allows you to use GTSAM + from within MATLAB, which provides for visualization and rapid development. + We discuss this next. +\end_layout + +\begin_layout Subsection +Using the MATLAB Interface +\end_layout + +\begin_layout Standard +A large subset of the GTSAM functionality can be accessed through wrapped + classes from within MATLAB +\begin_inset Foot +status open + +\begin_layout Plain Layout +GTSAM also allows you to wrap your own custom-made classes, although this + is outside the scope of this manual +\end_layout + +\end_inset + +. + The following code excerpt is the MATLAB equivalent of the C++ code in + Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:Pose2SLAMExample" + +\end_inset + +: +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/Pose2SLAMExample.m" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/Pose2SLAMExample.m},label={listing:Pose2SLAMExample-MATLAB}" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + Note that the code is almost identical, although there are a few syntax + and naming differences: +\end_layout + +\begin_layout Itemize +Objects are created by calling a constructor instead of allocating them + on the heap. +\end_layout + +\begin_layout Itemize +Namespaces are done using dot notation, i.e., +\series bold +\emph on +noiseModel::Diagonal::SigmasClasses +\series default +\emph default + becomes +\series bold +\emph on +noiseModel.Diagonal.Sigmas +\series default +\emph default +. +\end_layout + +\begin_layout Itemize + +\series bold +\emph on +Vector +\series default +\emph default + and +\series bold +\emph on +Matrix +\series default +\emph default + classes in C++ are just vectors/matrices in MATLAB. +\end_layout + +\begin_layout Itemize +As templated classes do not exist in MATLAB, these have been hardcoded in + the GTSAM interface, e.g., +\series bold +\emph on +PriorFactorPose2 +\series default +\emph default + corresponds to the C++ class +\series bold +\emph on +PriorFactor +\series default +\emph default +, etc. +\end_layout + +\begin_layout Standard +After executing the code, you can call +\emph on +whos +\emph default +on the MATLAB command prompt to see the objects created. + Note that the indicated +\emph on +Class +\emph default + corresponds to the wrapped C++ classes: +\size small + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/whos.txt" + +\end_inset + + +\size default + +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In addition, any GTSAM object can be examined in detail, yielding identical + output to C++: +\size small + +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/print.txt" + +\end_inset + + +\size default + +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + And it does not stop there: we can also call some of the functions defined + for factor graphs. + E.g., +\end_layout + +\begin_layout Standard + +\size small +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/calls.txt" + +\end_inset + + +\size default + +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + computes the sum-squared error +\begin_inset Formula $\frac{1}{2}\sum_{i}\SqrMah{h_{i}(X_{i})}{z_{i}}{\Sigma}$ +\end_inset + + before and after optimization. +\end_layout + +\begin_layout Subsection +Reading and Optimizing Pose Graphs +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/w100-result.pdf + width 80text% + BoundingBox 30bp 170bp 610bp 630bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:w100" + +\end_inset + +MATLAB plot of small Manhattan world example with 100 poses (due to Ed Olson). + The initial estimate is shown in green. + The optimized trajectory, with covariance ellipses, in blue. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +The ability to work in MATLAB adds a much quicker development cycle, and + effortless graphical output. + The optimized trajectory in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:w100" + +\end_inset + + was produced by the code below, in which +\emph on +load2D +\emph default + reads TORO files. + To see how plotting is done, refer to the full source code. +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/Pose2SLAMExample-graph.m" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/Pose2SLAMExample\\_graph.m},label={listing:Pose2SLAMExample-graph}" + +\end_inset + + +\end_layout + +\begin_layout Subsection +PoseSLAM in 3D +\end_layout + +\begin_layout Standard +PoseSLAM can easily be extended to 3D poses, but some care is needed to + update 3D rotations. + GTSAM supports both +\series bold +quaternions +\series default + and +\begin_inset Formula $3\times3$ +\end_inset + + +\series bold +rotation matrices +\series default +to represent 3D rotations. + The selection is made via the compile flag GTSAM_USE_QUATERNIONS. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/sphere2500-result.pdf + width 70text% + BoundingBox 60bp 150bp 610bp 610bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:w100-1" + +\end_inset + +3D plot of sphere example (due to Michael Kaess). + The very wrong initial estimate, derived from odometry, is shown in green. + The optimized trajectory is shown red. + Code below: +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/Pose3SLAMExample-graph.m" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/Pose3SLAMExample\\_graph.m},label={listing:Pose3SLAMExample-graph-1}" + +\end_inset + + +\end_layout + +\begin_layout Section +\begin_inset CommandInset label +LatexCommand label +name "sec:Landmark-based-SLAM" + +\end_inset + +Landmark-based SLAM +\end_layout + +\begin_layout Subsection +Basics +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/FactorGraph4.pdf + scale 80 + BoundingBox 50bp 590bp 290bp 710bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:SLAM" + +\end_inset + +Factor graph for landmark-based SLAM +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In +\series bold +landmark-based SLAM +\series default +, we explicitly build a map with the location of observed landmarks, which + introduces a second type of variable in the factor graph besides robot + poses. + An example factor graph for a landmark-based SLAM example is shown in Figure + +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:SLAM" + +\end_inset + +, which shows the typical connectivity: poses are connected in an odometry + Markov chain, and landmarks are observed from multiple poses, inducing + binary factors. + In addition, the pose +\begin_inset Formula $x_{1}$ +\end_inset + + has the usual prior on it. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/example2.pdf + scale 47 + BoundingBox 90bp 220bp 520bp 555bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:PlanarSLAMExample" + +\end_inset + +The optimized result along with covariance ellipses for both poses (in green) + and landmarks (in blue). + Also shown are the trajectory (red) and landmark sightings (cyan). +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +The factor graph from Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:SLAM" + +\end_inset + + can be created using the MATLAB code in Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:PlanarSLAMExample" + +\end_inset + +. + As before, on line 2 we create the factor graph, and Lines 8-18 create + the prior/odometry chain we are now familiar with. + However, the code on lines 20-25 is new: it creates three +\series bold +measurement factors +\series default +, in this case +\begin_inset Quotes eld +\end_inset + +bearing/range +\begin_inset Quotes erd +\end_inset + + measurements from the pose to the landmark. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/PlanarSLAMExample.m" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/PlanarSLAMExample.m},label={listing:PlanarSLAMExample}" + +\end_inset + + +\end_layout + +\begin_layout Subsection +Of Keys and Symbols +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + The only unexplained code is on lines 4-6: here we create integer keys + for the poses and landmarks using the +\series bold +\emph on +symbol +\series default +\emph default + function. + In GTSAM, we address all variables using the +\series bold +\emph on +Ke +\emph default +y +\series default + type, which is just a typedef to +\series bold +\emph on +size_t +\series default +\emph default + +\begin_inset Foot +status open + +\begin_layout Plain Layout +a 32 or 64 bit integer, depending on your platform +\end_layout + +\end_inset + +. + The keys do not have to be numbered continuously, but they do have to be + unique within a given factor graph. + For factor graphs with different types of variables, we provide the +\series bold +\emph on +symbol +\series default +\emph default + function in MATLAB, and the +\series bold +\emph on +Symbol +\series default +\emph default + type in C++, to help you create (large) integer keys that are far apart + in the space of possible keys, so you don't have to think about starting + the point numbering at some arbitrary offset. + To create a a +\emph on +symbol key +\emph default + you simply provide a character and an integer index. + You can use base 0 or 1, or use arbitrary indices: it does not matter. + In the code above, we we use 'x' for poses, and 'l' for landmarks. +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +, and use the resulting keys +\series bold +\emph on +i1 +\series default +\emph default +, +\series bold +\emph on +i2 +\series default +\emph default +, +\series bold +\emph on +i3 +\series default +\emph default +, +\series bold +\emph on +j1 +\series default +\emph default +, and +\series bold +\emph on +j2 +\series default +\emph default + to create the factors in the correct way. +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +The optimized result for the factor graph created by Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:PlanarSLAMExample" + +\end_inset + + is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:PlanarSLAMExample" + +\end_inset + +, and it is readily apparent that the landmark +\begin_inset Formula $l_{1}$ +\end_inset + + with two measurements is better localized. + In MATLAB we can also examine the actual numerical values, and doing so + reveals some more GTSAM magic: +\end_layout + +\begin_layout Standard + +\size small +\begin_inset CommandInset include +LatexCommand verbatiminput +filename "Code/PlanarSLAMExample.txt" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + Indeed, the keys generated by symbol are automatically detected by the + +\series bold +\emph on +print +\series default +\emph default + method in the +\series bold +\emph on +Values +\series default +\emph default + class, and rendered in human-readable form +\begin_inset Quotes eld +\end_inset + +x1 +\begin_inset Quotes erd +\end_inset + +, +\begin_inset Quotes eld +\end_inset + +l2 +\begin_inset Quotes erd +\end_inset + +, etc, rather than as large, unwieldy integers. + This magic extends to most factors and other classes where the +\series bold +Key +\series default + type is used. +\end_layout + +\begin_layout Subsection +A Larger Example +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/littleRobot.pdf + width 90text% + BoundingBox 0bp 200bp 612bp 600bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:littleRobot" + +\end_inset + +A larger example with about 100 poses and 30 or so landmarks, as produced + by gtsam_examples/PlanarSLAMExample_graph.m +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +GTSAM comes with a slightly larger example that is read from a .graph file + by PlanarSLAMExample_graph.m, shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:littleRobot" + +\end_inset + +. + To not clutter the figure only the marginals are shown, not the lines of + sight. + This example, with 119 (multivariate) variables and 517 factors optimizes + in less than 10 ms. +\end_layout + +\begin_layout Subsection +A Real-World Example +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/Victoria.pdf + width 90text% + BoundingBox 0bp 0bp 420bp 180bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:Victoria-1" + +\end_inset + +Small section of optimized trajectory and landmarks (trees detected in a + laser range finder scan) from data recorded in Sydney's Victoria Park (dataset + due to Jose Guivant, U. + Sydney). +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +A real-world example is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:Victoria-1" + +\end_inset + +, using data from a well known dataset collected in Sydney's Victoria Park, + using a truck equipped with a laser range-finder. + The covariance matrices in this figure were computed very efficiently, + as explained in detail in +\begin_inset CommandInset citation +LatexCommand citep +key "Kaess09ras" + +\end_inset + +. + The exact covariances (blue, smaller ellipses) obtained by our fast algorithm + coincide with the exact covariances based on full inversion (orange, mostly + hidden by blue). + The much larger conservative covariance estimates (green, large ellipses) + were based on our earlier work in +\begin_inset CommandInset citation +LatexCommand citep +key "Kaess08tro" + +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +Structure from Motion +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/cube.pdf + width 80text% + BoundingBox 60bp 90bp 612bp 380bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:SFMExample" + +\end_inset + +An optimized +\begin_inset Quotes eld +\end_inset + +Structure from Motion +\begin_inset Quotes erd +\end_inset + + with 10 cameras arranged in a circle, observing the 8 vertices of a +\begin_inset Formula $20\times20\times20$ +\end_inset + + cube centered around the origin. + The camera is rendered with color-coded axes, (RGB for XYZ) and the viewing + direction is is along the positive Z-axis. + Also shown are the 3D error covariance ellipses for both cameras and points. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + +\series bold + Structure from Motion +\series default + (SFM) is a technique to recover a 3D reconstruction of the environment + from corresponding visual features in a collection of +\emph on +unordered +\emph default + images, see Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:SFMExample" + +\end_inset + +. + In GTSAM this is done using exactly the same factor graph framework, simply + using SFM-specific measurement factors. + In particular, there is a +\series bold +projection factor +\series default + that calculates the reprojection error +\begin_inset Formula $f(x_{i},p_{j};z_{ij},K)$ +\end_inset + + for a given camera pose +\begin_inset Formula $x_{i}$ +\end_inset + + (a +\series bold +\emph on +Pose3 +\series default +\emph default +) and point +\begin_inset Formula $p_{j}$ +\end_inset + + (a +\series bold +\emph on +Point3 +\series default +\emph default +). + The factor is parameterized by the 2D measurement +\begin_inset Formula $z_{ij}$ +\end_inset + + (a +\series bold +\emph on +Point2 +\series default +\emph default +), and known calibration parameters +\begin_inset Formula $K$ +\end_inset + + (of type +\series bold +\emph on +Cal3_S2 +\series default +\emph default +). + The following listing shows how to create the factor graph: +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/SFMExample.m" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/SFMExample.m},label={listing:SFMExample}" + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:SFMExample" + +\end_inset + +, assuming that the factor graph was already created, we add measurement + factors in the double loop. + We loop over images with index +\begin_inset Formula $i$ +\end_inset + +, and in this example the data is given as two cell arrays: Z{i} specifies + a set of measurements +\begin_inset Formula $z_{k}$ +\end_inset + + in image +\begin_inset Formula $i$ +\end_inset + +, and +\begin_inset Formula $J\{i\}$ +\end_inset + + specifies the corresponding point index. + The specific factor type we use is a +\series bold +\emph on +GenericProjectionFactorCal3_S2 +\series default +\emph default +, which is the MATLAB equivalent of the C++ class +\series bold +\emph on +GenericProjectionFactor +\series default +\emph default +, where +\series bold +\emph on +Cal3_S2 +\series default +\emph default + is the camera calibration type we choose to use (the standard, no-radial + distortion, 5 parameter calibration matrix). + As before landmark-based SLAM (Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Landmark-based-SLAM" + +\end_inset + +), here we use symbol keys except we now use the character 'p' to denote + points, rather than 'l' for landmark. +\end_layout + +\begin_layout Standard +Important note: a very tricky and difficult part of making SFM work is (a) + data association, and (b) initialization. + GTSAM does neither of these things for you: it simply provides the +\begin_inset Quotes eld +\end_inset + +bundle adjustment +\begin_inset Quotes erd +\end_inset + + optimization. + In the example, we simply assume the data association is known (it is encoded + in the J sets), and we initialize with the ground truth, as the intent + of the example is simply to show you how to set up the optimization problem. +\end_layout + +\begin_layout Section +iSAM: Incremental Smoothing and Mapping +\end_layout + +\begin_layout Standard +GTSAM provides an incremental inference algorithm based on a more advanced + graphical model, the Bayes tree, which is kept up to date by the +\series bold +iSAM +\series default + algorithm (incremental Smoothing and Mapping, see +\begin_inset CommandInset citation +LatexCommand citet +key "Kaess08tro,Kaess12ijrr" + +\end_inset + + for an in-depth treatment). + For mobile robots operating in real-time it is important to have access + to an updated map as soon as new sensor measurements come in. + iSAM keeps the map up-to-date in an efficient manner. + +\end_layout + +\begin_layout Standard +Listing +\begin_inset CommandInset ref +LatexCommand ref +reference "listing:iSAMExample" + +\end_inset + + shows how to use iSAM in a simple visual SLAM example. + In line 1-2 we create a +\series bold +\emph on +NonlinearISAM +\series default +\emph default + object which will relinearize and reorder the variables every 3 steps. + The corect value for this parameter depends on how non-linear your problem + is and how close you want to be to gold-standard solution at every step. + In iSAM 2.0, this parameter is not needed, as iSAM2 automatically determines + when linearization is needed and for which variables. +\end_layout + +\begin_layout Standard +The example involves eight 3D points that are seen from eight successive + camera poses. + Hence in the first step -which is omitted here- all eight landmarks and + the first pose are properly initialized. + In the code this is done by perturbing the known ground truth, but in a + real application great care is needed to properly initialize poses and + landmarks, especially in a monocular sequence. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\begin_inset CommandInset include +LatexCommand lstinputlisting +filename "Code/VisualISAMExample.cpp" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/VisualISAMExample.cpp},label={listing:iSAMExample}" + +\end_inset + + +\end_layout + +\begin_layout Standard +The remainder of the code illustrates a typical iSAM loop: +\end_layout + +\begin_layout Enumerate +Create factors for new measurements. + Here, in lines 9-18, a small +\series bold +\emph on +NonlinearFactorGraph +\series default +\emph default + is created to hold the new factors of type +\series bold +\emph on +GenericProjectionFactor +\series default +\emph default +. +\end_layout + +\begin_layout Enumerate +Create an initial estimate for all newly introduced variables. + In this small example, all landmarks have been observed in frame 1 and + hence the only new variable that needs to be initialized at each time step + is the new pose. + This is done in lines 20-22. + Note we assume a good initial estimate is available as +\emph on +initial_x[i] +\emph default +. +\end_layout + +\begin_layout Enumerate +Finally, we call +\emph on +isam.update() +\emph default +, which takes the factors and initial estimates, and incrementally updates + the solution, which is available through the method +\emph on +isam.estimate() +\emph default +, if desired. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +More Applications +\end_layout + +\begin_layout Standard +While a detailed discussion of all the things you can do with GTSAM will + take us too far, below is a small survey of what you can expect to do, + and which we did using GTSAM. +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status open + +\begin_layout Plain Layout +\begin_inset Float figure +wide false +sideways false +status collapsed + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename figures/SAM/FactorGraph.pdf + scale 80 + BoundingBox 40bp 600bp 300bp 792bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:Filtering" + +\end_inset + +Factor graph explanation of Filtering. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +The factor graph for landmark-based SLAM is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:Filtering" + +\end_inset + +. +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Subsection +Conjugate Gradient Optimization +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/Beijing.pdf + width 70text% + BoundingBox 100bp 240bp 500bp 550bp + clip + +\end_inset + + +\begin_inset Caption Standard + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:Beijing" + +\end_inset + + A map of Beijing, with a spanning tree shown in black, and the remaining + +\emph on +loop-closing +\emph default + constraints shown in red. + A spanning tree can be used as a +\emph on +preconditioner +\emph default + by GTSAM. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +GTSAM also includes efficient preconditioned conjugate gradients (PCG) methods + for solving large-scale SLAM problems. + While direct methods, popular in the literature, exhibit quadratic convergence + and can be quite efficient for sparse problems, they typically require + a lot of storage and efficient elimination orderings to be found. + In contrast, iterative optimization methods only require access to the + gradient and have a small memory footprint, but can suffer from poor convergenc +e. + Our method, +\emph on +subgraph preconditioning +\emph default +, explained in detail in +\begin_inset CommandInset citation +LatexCommand citet +key "Dellaert10iros,Jian11iccv" + +\end_inset + +, combines the advantages of direct and iterative methods, by identifying + a sub-problem that can be easily solved using direct methods, and solving + for the remaining part using PCG. + The easy sub-problems correspond to a spanning tree, a planar subgraph, + or any other substructure that can be efficiently solved. + An example of such a subgraph is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:Beijing" + +\end_inset + +. +\end_layout + +\begin_layout Subsection +Visual Odometry +\end_layout + +\begin_layout Standard +A gentle introduction to vision-based sensing is +\series bold +Visual Odometry +\series default + (abbreviated VO, see e.g. + +\begin_inset CommandInset citation +LatexCommand citet +key "Nister04cvpr2" + +\end_inset + +), which provides pose constraints between successive robot poses by tracking + or associating visual features in successive images taken by a camera mounted + rigidly on the robot. + GTSAM includes both C++ and MATLAB example code, as well as VO-specific + factors to help you on the way. +\end_layout + +\begin_layout Subsection +Visual SLAM +\end_layout + +\begin_layout Standard + +\series bold +Visual SLAM +\series default + (see e.g., +\begin_inset CommandInset citation +LatexCommand citet +key "Davison03iccv" + +\end_inset + +) is a SLAM variant where 3D points are observed by a camera as the camera + moves through space, either mounted on a robot or moved around by hand. + GTSAM, and particularly iSAM (see below), can easily be adapted to be used + as the back-end optimizer in such a scenario. +\end_layout + +\begin_layout Subsection +Fixed-lag Smoothing and Filtering +\end_layout + +\begin_layout Standard +GTSAM can easily perform recursive estimation, where only a subset of the + poses are kept in the factor graph, while the remaining poses are marginalized + out. + In all examples above we explicitly optimize for all variables using all + available measurements, which is called +\series bold +Smoothing +\series default + because the trajectory is +\begin_inset Quotes eld +\end_inset + +smoothed +\begin_inset Quotes erd +\end_inset + + out, and this is where GTSAM got its name (GT +\emph on +Smoothing +\emph default + and Mapping). + When instead only the last few poses are kept in the graph, one speaks + of +\series bold +Fixed-lag Smoothing +\series default +. + Finally, when only the single most recent poses is kept, one speaks of + +\series bold +Filtering +\series default +, and indeed the original formulation of SLAM was filter-based +\begin_inset CommandInset citation +LatexCommand citep +key "Smith87b" + +\end_inset + +. +\end_layout + +\begin_layout Subsection +Discrete Variables and HMMs +\end_layout + +\begin_layout Standard +Finally, factor graphs are not limited to continuous variables: GTSAM can + also be used to model and solve discrete optimization problems. + For example, a Hidden Markov Model (HMM) has the same graphical model structure + as the Robot Localization problem from Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Robot-Localization" + +\end_inset + +, except that in an HMM the variables are discrete. + GTSAM can optimize and perform inference for discrete models. +\end_layout + +\begin_layout Section* +Acknowledgements +\end_layout + +\begin_layout Standard +GTSAM was made possible by the efforts of many collaborators at Georgia + Tech and elsewhere, including but not limited to Doru Balcan, Chris Beall, + Alex Cunningham, Alireza Fathi, Eohan George, Viorela Ila, Yong-Dian Jian, + Michael Kaess, Kai Ni, Carlos Nieto, Duy-Nguyen Ta, Manohar Paluri, Christian + Potthast, Richard Roberts, Grant Schindler, and Stephen Williams. + In addition, Paritosh Mohan helped me with the manual. + Many thanks all for your hard work! +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\begin_inset CommandInset bibtex +LatexCommand bibtex +bibfiles "gtsam" +options "apalike" + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf new file mode 100644 index 000000000..240528179 Binary files /dev/null and b/doc/gtsam.pdf differ diff --git a/doc/images/Beijing.pdf b/doc/images/Beijing.pdf new file mode 100644 index 000000000..57f11822f Binary files /dev/null and b/doc/images/Beijing.pdf differ diff --git a/doc/images/FactorGraph.pdf b/doc/images/FactorGraph.pdf new file mode 100644 index 000000000..13b0c494b Binary files /dev/null and b/doc/images/FactorGraph.pdf differ diff --git a/doc/images/FactorGraph2.pdf b/doc/images/FactorGraph2.pdf new file mode 100644 index 000000000..df8d47e5c Binary files /dev/null and b/doc/images/FactorGraph2.pdf differ diff --git a/doc/images/FactorGraph3.pdf b/doc/images/FactorGraph3.pdf new file mode 100644 index 000000000..affa329b3 Binary files /dev/null and b/doc/images/FactorGraph3.pdf differ diff --git a/doc/images/FactorGraph4.pdf b/doc/images/FactorGraph4.pdf new file mode 100644 index 000000000..cba5ad2a3 Binary files /dev/null and b/doc/images/FactorGraph4.pdf differ diff --git a/doc/images/Localization.pdf b/doc/images/Localization.pdf new file mode 100644 index 000000000..aded83c63 --- /dev/null +++ b/doc/images/Localization.pdf @@ -0,0 +1,101 @@ +%PDF-1.4 +%쏢 +5 0 obj +<> +stream +x͘ϊ$7 uLQlKw gg  a&׏,Y!,Ȯ,~ݵOyu 1aٓJߦK"~bRȪ?m$@!Cʼ(1G¶>]QdiysW?}>=3ob}>^K BNPN9(a߸並_^8J^؎ {%H_ۯ/)URM` pMEn V唨d-T TATPŕȸB*`TQ&єIB (9/L^Rd T TA`Y]%t)ZTQ&єI䴺l]R MD ե`]*Q0 0ML%B '0D(hJDxD+qRKڈ+Mڍg]RED3-j*E^qi6ž_ jUV7bH +-Q2ahןVh ͨd "o 4 9TDP%P6AF~iĹuD+@k( ьD&7硤Py-lNi`2 +n0ͨdv40M XDD%D6^"wN4 LDe4&ץ</GT}PTHYн}+B3VtN[O`#g< +Dxħ`>?y@cJjDzF\Qe^).{c*r(Or[F(YQ<<"R%FQNxډdSEh%4՞?=e>ҵ]_rȭ&=>MsqqT\YD9v^|Eԕ +g%ynBFW+DU~O%Ϸ=a殤61NA(X9b0Ql|賱bO<z2iV /AaNZzqO%/}kqG^TP> +/Contents 5 0 R +>> +endobj +3 0 obj +<< /Type /Pages /Kids [ +4 0 R +] /Count 1 +>> +endobj +1 0 obj +<> +endobj +7 0 obj +<>endobj +9 0 obj +<> +endobj +10 0 obj +<> +endobj +8 0 obj +<> +endobj +11 0 obj +<> +endobj +12 0 obj +<>stream + + + + + +Artifex Ghostscript 8.54 PDF Writer + +/private/tmp/tpfd5bb1cf_55d8_447e_8f97_d6b24a262922.ps + + + + + +endstream +endobj +2 0 obj +<>endobj +xref +0 13 +0000000000 65535 f +0000001664 00000 n +0000003341 00000 n +0000001605 00000 n +0000001446 00000 n +0000000015 00000 n +0000001426 00000 n +0000001729 00000 n +0000001829 00000 n +0000001770 00000 n +0000001799 00000 n +0000001909 00000 n +0000001967 00000 n +trailer +<< /Size 13 /Root 1 0 R /Info 2 0 R +/ID [<0E15732D337F57A9CB2C0954A12E8EF8><0E15732D337F57A9CB2C0954A12E8EF8>] +>> +startxref +3722 +%%EOF diff --git a/doc/images/Odometry.pdf b/doc/images/Odometry.pdf new file mode 100644 index 000000000..a9adf2ced Binary files /dev/null and b/doc/images/Odometry.pdf differ diff --git a/doc/images/Victoria.pdf b/doc/images/Victoria.pdf new file mode 100644 index 000000000..0fa9b9220 Binary files /dev/null and b/doc/images/Victoria.pdf differ diff --git a/doc/images/cube.pdf b/doc/images/cube.pdf new file mode 100644 index 000000000..6314d7e4e --- /dev/null +++ b/doc/images/cube.pdf @@ -0,0 +1,559 @@ +%PDF-1.3 +% +4 0 obj +<< /Length 5 0 R /Filter /FlateDecode >> +stream +xUK0 D=ŜiGXvE*D$~3+DQY1Z*A4 .uVG [- UR ")ApYpzW> +endobj +6 0 obj +<< /ProcSet [ /PDF /ImageB /ImageC /ImageI ] /ExtGState << /Gs1 11 0 R >> +/XObject << /Im1 7 0 R /Im2 9 0 R >> >> +endobj +7 0 obj +<< /Length 8 0 R /Type /XObject /Subtype /Image /Width 1539 /Height 972 /ColorSpace +12 0 R /BitsPerComponent 8 /Filter /FlateDecode >> +stream +xYtUusvu[FQ..++}! !BZEP +6`"" =H}gM{o"|sdf9?k.VoES@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@P@ gq=rx^p|?X7+ +( +( +( +4=9h <cyx!%:FP/Mo"P@P@P@P@I>MnWN2(vBBW x ޅCS%\ׄhL7a2ŪEP@P@P@Pi4$Yì>^aT6CK)&x'|d| 3`&|~\¯pӧq466j[ +( +( +( +ؚC}_QoR + y|ŧ)'ӱ\:X K`)R%Y `N͋vR+ +( +( +( +[3l)di_q1J1ќ +y-ByLldnEI&Yd a=lubY.9,e$)M- Ϯ +( +( +(loRiɧeۊWS/"*>fSɲBbcM)f=LҐ,6沮Y֜EYXؙeנsP@P@P@P \,aw)˘[δ|PּP2ƶR,">+LG]+XhH#Lve;=Yɦ!/]_u%jɲVԵВ%/伥AiP@P@P@P҆26T _UQ[ުdB%#+՚L,criz"feyac6۲ؕ,fs ls9ǁ|Ђ--Y_ +VVv,fm;6aW9GZX`t +( +( +( +-QɊ*vWkUmJoë]Δ22XZ65]91iP> +8ԌChhJoǪ,H] K+;{8^jto^wUP@P@P@4 bC5 ў:V5Q*&eR%Wi93RYќؚϮP,p,cym"p1SliφNŠ,N]V<ڞlACvtYЃvܹ +( +( +( +(pnx% Yс9#w<_ͫռSJrQ_š"6a_ ?x' 8UBX G8Xj6ueC|=Jc~ '6tY~}>P@P@P@xTrMaVGħ5[˝Չߑ7:~5Vufq+KY_"vryT>q99QVP;nl~1=>,y[1lcϽU@P@P@P@*$*ӑ5_Wڅ703jӉ:f>*dA_kJTLC +9J85\sΔrm8֖9PÞx-k/KQ+6pQ.W'S@P@P@P@GFwNj݅i]ywe\u^Zt% +X^κl-aW +9Z U1*gC}i.q˅\.r aD'tablgX7J0ꇱj8u(s9NA^* +( +(Z`|0F³z~m|Gb%; SfWwaxgFw&u|VU̯dIXƎRs3͸ϕf\)J1WJؚsm9݁9ҝ0)Xov=ɶ8uY54Ս`(Vbh`Ps\y̷bVP@P@P܂aKXbnc +4#gFCHD|׌۰ӟ]ҍI]y1n46V[WMUR_ljΖ(Xg (]iy:q+GJA{gg_OZ7UhuX:c~[ưc$qr$ct _P@P@Z WZsEaO!:YO13z^0^zq]У<DO30Fx܅{ pϓx&ĔDbR"b"14HH_9-H,H$p͉ĮD18HO$Kڿ%AP)[q-gs3ǻsa{9:ூ1q<[Ʊc4Frr{p, +( +( +(}$0ޙ+m9׊-f[s,Y|$&y#I%$& !B! x.B +LSJyvb:yYFW^΄WjxS:E5XPɲ +֕ݥ4x!Cwxh؀ A˹8ܓÎcEPO׊sP@P@P@AyZ~xk=ܑk2 Ybj&of2>ň)fDI+Bjw(8bXi;Mc5;] _ufJ&u a\nߞ/m[aykֵbk)9Xĉ{m*.TA] Q<Ξx0C=Z5?A7+BAjNP@P@PLѐtu\0Qڰ#vEmy|ûټŘ,d?œ)JLA@F@AH~U@kq +NYh:̉SUv?vr5ԑQެI?c;JGjJVue hO18ٌ y\ɧjN\ APp9փC=5gK1ɢQԏ!MаǗ{ +( +( +DS^#M,QontR :rmޖ,\^axf//16!9 ,e2$ `l2]"72>1|c9$3ْKC{sXµAwd)YŲv̪Ӂ:0cr{ުfJ; +l)fW\.x: +](KAqGqa=Ʈ>lY?5X>%q|,kDZya( ?5n +( +( +(͈ˁ^'K4(=.ZO.D7;#Y]ɒ +U)B&b>#e34)ƦLW'?SS;PX˺fl.=e4h%ۥG]m8H܄8ߚ=YUɜLk{ռZjFV3IU|P|SN-E,@G9MJ,*]n{'r{[EV;& p0N`~*&V1my+\'|]RMьtvzc?>5:qgZruױ(;`SlxX9! ~8F~4[Fs$95Kҭݛ +( +( +(д@_}D4En r-rgzхXU*aF9b,BF\X˘^L^K1)NB!PH7#0M|Lf2EYQĺ6Pޜ+Os#L4ܦ},=x[w8=ޒrffj&UBV0W[N9199ѐϾ\p2+ڧF$r8^jݓle D@U@$uԅS<7guM dMDmϰ{s)tG9^~vJbQOd8>o0L.s!΂_ğ᫄_#P@~>={o. +*T }L {h YV|_Ŧpۍ]7rt'Ώx!Lx +4-`Ns>iL(fx1c[Jo1/ .~[YldW)x8Ǒf3˗+ifK aVq/+f6xtG&s4r{(̅BOB%,fa. rXR, ?S׌t"(£W ! +W]W +(@S͍[͂&W* LFSѲT4;6 RNPtMn,̜d +jū#37ˁry5IYɇt9􈰛@! +_.l3+aQ Z^Zׁ]0;co_h-'qz s50}H>'~7ߓ<*0^!c!/6KO^13!0RlH-]M8<6@ dG9*ԞkXݝ'uXx:hÓl}=9܋3ݹV{I wqcnυS<'| +ZP R|oZҒU~]QM BOQYН(P@?g֣~kMJP^ +%ڗmʌ2oRg}P%ќx>!-ɢ>^cDc o2%/#b$`'ރaؾ籷]u[-kۧGe. o?̪b[z!vT ۘūr{(TœPR{*aMk[ߗ>ԅ" 5N +qPN!>9ް4=sP&ǯ ͇(%YڬhaV43}BKqa7r)gP(a]ueA :E;>Lw PƗDžA +x>x-7sx73$ŴPLw:] aP@VYPV,hv +wec֛ qdrjg'rm.OA4hiA@vA麸0}ސ,ejdq*aQ.X asMJ;lv汽[Z%k[-VvM7u ;qK-M܃yɅ^0Yd AP}Y C)4/\+65+3ldK2n,cc1XSVY;¥ +( +w6{;@! +49#Ѷhev 3MMFoVC,gsI>zì̯afU1yJYfry"(ipˆ0X]KUg"[ҁ'{;0/hȩW9&G\ƍ0D z..7(-|'dHqI^M.|Dwzv +B,NC-=,lfs +Yۂ-oE]kaEU:hc5 ߖ\nυ~~;c>,L6l,MG=[ڰ5نeG;vT l$[-܌yLoLEr% +(` ͇(д.LEsYT4=}L5CP˾iШu][Ì|VU+e q\,fi/Rro9(,3ߕ:|)0R ZK:;acoc3LKz3r~ +>tn̊  ZGu|5A 43 E `< oq=Ch{L8(2NXka]8皯]s[:U@"+pB/@! NtԳmՖ=U K5{۳/bOvfW+v{`{![٘Ú=B} +hs +(MM>w_AwChZWgE볣hn*KFaB\ƹП?}8[zC,®̮az>J&Rc@q99ʁrd0'ckx8̊p.Nu9oP@}1u,bY'V Q6aePGq©8>?\(-ˁ6mi_n-$B_c MNGAP(M)ad PLR,J7aY mH-`/Bnc93c9$;{QwAwChZT&եYѧq\˹|$;zQV?NLfJ[ޮVPn4^' ŔT<.,C (y!dahf~fy6Yˊ,mI]gyV?vfqd'r]N\oQh+QHI@x:#qP(cP8UDHaXH"ޜqx +ܞ ] K@P̍IfҐ@)ZsJw>?>.\r+h1s(9dG<^Xg\3fA +(P`S| +4!k!?Ehf2)0NAMhpP@~K!Ϭ@7W_~ϟ;egh_At0:]"o^٤  X <ʿ$ĕts%'D@7Rʍkk\z.ײ%"+a^ÁŶlm;9Ϯbz>.grK_fKA/gzA>?L|khX + YTv,Qaol)uN }i\BTGh[h5D 9wARE<}| 018WڞtBOD[ʁ (] fq#ɵۂ#( Y/z/W@~>[cwM?Ob['7'+!Q\A@APw6԰1|܊ɥL,f|sF0<9Lo'>K[Ĝ ¬assA⊠tTʂ2jaI;װzI98Ozsq#}_ %ڔ=St^t :G}uX{ъ(ow} ,;a7N9 7PӜgk] ݁s Dž\ϳ +VAۇ0U}6o(wLPnL37ݍ-9P +$O{ù439O'ۻ#+bN%L,b|a:ǘlgzFߛmŸ+&61;L0'+b^ Z2U0fEg<ԗϰcp:jǑ81os=NgD_lD=挨!ˊD#/6EM@_ cBO( + +bp!dAtaWV:`-?<Rq+y\IOv6Kq(ɾ$a "#(|^<4 7z\ +(+w++po~:[7s8ޗa=X\|U'ׂ/`T.óbB2[09Fݜ8,A&69gN!sZ0yekͼ6, }܏9}#92f\nNߡK͹ԌK\|grӍPRN%5I$脊AjP@d+d%]-7y׉Fqcq bC4tfS;TyQʧżל7 +,LL}tXf%ɷq4-Sœ2fn%ua\Xjuq$q9 ӗpy`ٓEC&SL(|ϠЕA]{}"P@Zcp?}n:5hpGaowjA +˙ߒ|VLFLdx^qQP(ο9wX֢6137)fV. +9mKf+W=kP?>Gμ72smzz”aqE6}DG8Yo~(»$plE]i0.C= '9p&=r8˩t8՜Ӆ*D!GqyeW YlbM$%-C > +(},';)po܈ZG@vv`S[֕S_"ao3 hH2=Ap;VQP(οUIf׹|BfM +[XWlE[?qxpz<_ҤtEPOOC;fE85<<|Z u}MK}\j7/ۭpZ2YL+`J1 aqK1o)6e/SLbz.E|]. +Z؊ebCG6wc{Ov=NW{Yhξ04аxhX +CBEDcgg YAzǟaVS2,.(ID;>{BOB/ M46@ۜ݅(`[[XXbNCq +>< +(IVsiVOamɵJNUr/eeBcj6oewT{yÞǞqX֞g=xcݙǻ4MhrB  B!@2( !!P@B$9g=W]*[UoT]us>{7`f3lWz礃؋F b jAܜMw,F^,NÒt,gX)kcsvv^?H _+OƳ(yeob6?Df7c5l'cA?eP!(M4zlу%m2-gkB0ep6(<|tw,ׇq0?`o6vfaK&6&obyTjZJ@ (%ETj,=H xn}" ?-BlؖuXea|na&-(XؼaA:e`qa +R쌽=qnhKOJTD[¬aK!_Co{Z@%p_/+= q 6ai5=uAl +aK:g`W&dR\|G^Ǟ\llƺLvAsǀ'UJ^%WJ@ ( +QKZ7͍۪ey_p20M!,i A Gv,9E(iѼ " +Ay ַŶvSݨ'0\+P<!A(̓|Ya,6!Ѓ7"S|Z!=ÀFvA61OCXalll|va{>w[bُXUn&e!, bÈxejgUJ@ (%V J6ɢV19qIr˙<-7aD3!v:xjAA|O &pgq~.?cP*B4WِEc}*M$$g +A لZFL>x'&|!|UawY +>dž|wM^9n/eƂ> Jm*$hJ@ (%@ dd%ϲ\,7w{G|Gѿ8sS/zާ FNNJ01gpwĩn8(QA!h*j3Z;Fb QID@$jLʽx]} +G> ^ËƗX醡Գ*ϱq>V0UAtdc UAohhĝAܫPJ@ (%Tz`z%KMDh ߑQAtEi)̦|Iw4lLǮ0fhNNWl0ʇj4jƣiOlDVw%B]ID srݸS5r yXƢLD \O>/r +s 2me&>cVoR#g&iUPJ@ +A~l-/ D~99Vt61M5=@Aǀ8dδ=QCP@<>z)ߊՔ@KRG=<{6勗l;ix?Q&f|88elx=Vhut7kZꕕPJ@ +RJ^@݀o3|i5ĽZ1B+;x+ HNf\AI;TtBMwìE#J(*d"ݽo9 xSvA +;foeblgcϷf&^+aŒ خ2,8e2[ί*%P-A@TA GXMƑit/@(0XlvJPd+{,."uDwfRS}nexwfX9/4Ӄx!/]8Kx!υ1=it +GFb>\WJ@ (%bN@#*$#Т,1V') D2IIO+Oޅ0C=‡SmL jϔ7krSCD/0vs0YML}rT (%-bSϦ@KjH>Z!ݍ[z$ 4 DwƗ.&@y~ŘrW#⿯F.q1nH%PJ VTI=HVqX6oő^H a$""-s+r`.0x>|_KgqάRJ@ (% +APϠ@|VHV@tϐ9-^`O̱Գ)%h6NR@<-H:.QӺ֫'nUӡTJ@ (%A@f@ӯ(!y;!-IӚƓ@tg5ZJ >^^J@ (%pTJ&+xhy{:k)Ƕʨi^[Jn-DF˱3+%hKVFq[Zj}[-ݵFMKTMϙn-Z_-WOPJTj<+=R Vsy-U![j+VJOooq8@K(%/HPLu'_r[kݛMںٵ/[:mꮣ#nBJ@ (%A@{я@ÌV+)Z&a4zpƳ::I[PJnT}_ (عfiev%p_O [HA~;ёNP>$BUx L&c@Umu߲nu8Z%!pKR_([:j*J@ (%n!B-@(o빸i8nm\/S (!7r U(ٻf~X${kb@o'US$>?ut$~W(%@2P!(Zu뇺Ψl|\0N@Xgkm̱&"0x zD/ `0 + ToC$>d-h-+_/3mΩgDPH@m 3!!! C P_B4 X|UJ麯V4IZq%t B7L wJ@ (%TJ]2.U4ȣ BP'TBPNg`;ұ!UA, `7IF/X!AE z= L^i+ N@'BBqJ.6-@ꪧ/'uhɔPJ eUJsu?/v&c'֕o:@W| @P"Bн$gd[A,́y222Q7FG\is8ؒӰXQy; /w|g=PdT;cO'2Zюh% 7 ١r\ѩUP2 )o%\]g ЙR2vRYR$N}]!N 2J@ (%p*JU!!!! !!yӐ!Q;}P:Rf0} {l5FZ>b \wKЎoLz=%EQ&u=HJ hqs6!h"iC!/#.llV1eHϛ(Ќ=ą;Xt>_^c? A66n#6NL+]=QCQ5 +5Ov2g@S6i~d#d0Ag =K /P +"O +[U=qu:FZ%nwiXzI%@P!G%xAd?$=Dt9d* B^"~+EY˅8Uy؝->+= {$5BP1w<#H1ѤSUƗ cMl`g8,\hK]p7n  :F~}j+W,n9 GS(>K o\;G_8j9BKö iZ^ (%7olK3z#sR?NIMw) %yr5]4 +!2 zjb{JWmp( +#͋ AÀ~# 񝇁ǀ +A3w vP0eXW$e7zd+?'Q32ΈA65QѺ@!((eè'n$뽭Q- $A-Fqs1ЌS$GiF+J@ (":VP!(V$[a0 +AtF(HO;yz%/ DğyoF UzzqDǥ!&R#4!ߛ[&W!Q;FzDW 3B-|h38y hBO&DWh%&؞8KY(Eig*Ǣ)LG+gh +V(^1eZ.W(IeԶNq$ċ@Ҍxܟ@tܿ[$L-JϬjA*mX"DސiR;NjJ56R)W? Aی]rcc'P=5ϣuԿgR}ƸFlȎ ʩ4W*ɒ + +Aݤz=zW*Ay +D-_Oדrɮc~ϕP M@nsEfL%1Hi\$u@3~knʰI1(~WB?ڹZ͎,5A,9om +u #"üA=eLh8ю - +4|L8V:;2jԾ:Ff08[ui-]~ ˉ )YR/NL@t~@J@DH╺=IEWJ@ & >w{e婚-J +"DFJ]?$RKFg ¨+z♄* +AP<|?ꁣqBPvfbSV!^(1S#ESi 5zO +All68Rp ~('Q j^B훨 )i!O!(S.dKqA+|׋q5%t}h5Vn9k=kkD>y_%"RL[6CPW掯Ipa\}p`[OBW AB)~a HёL7ƖXXo8'p.FWsWyB[zzQZ d. zJUHG"cDkz{gJ?<ώ@5?o򍨜:0GKYˠh +O{INE#y^I"P28$rA?!V@A+nrI(Q0.^8;Pec=BXE]â #B /@! +A^Lʰ6Iwewde!v 9#B(u]M"@G:9#lTr W/M:K?& +\m yg^q449Fk'&~/z%@ ܢ5拍wdcǴZ-m'D./r=,QzP6% ZyBP{j}Tlm_F A/*k +At +E; `%2! ;s éN8tsrk˨}gnDCr"_εf7-8"oi-@WOy':9}O 4 8i9yc"@!>oj;Є:J|8LX:\jOP)K +O#KY>"EfWʳZXΥQ[лv LeWP,*B(kCqi©.8F!(?fBІ(}ac!0䂧z0_ičcF67b|| +yօ9;y8S.ǵP*Ơj[WPK _Cl3ώ˙VwR_jGL0GSEA'Z#A 0!/&-ho,,[Pb8ZqG  S߆IZv%@RhӘcFBWbfcnT?VϨlKW$q1ƸN (%:)^$A/rjdt9M*[vB6@V@@2KLT@$ÍX=gT0օ\e~^7BSc&CwelY7^b `c:eaOXGꆳF: %PkEVb YN-!Y FTuځR7Z()pItf)@?h%Ԥw xB-F\r27y +A/oO@i[FJ+T޷M b'ƐbVoZ0% {K=T]$Yc4P)r$S.\+l]*u"F!Aq#@vgbskC&&c&_dbw\!ȳ ؖ0XblJ,á!\\E'C5gQkDÐspRF! Ǥ_1IEP|zNY3V!BЫ"?[_Z-<A\O(WyA&XQOڵ"-~w n|sB@ORBHyP{Qc7M,2BPubHe_@n&Խ_UzaLd,N!( 2CkBX?5Bh!c D +AK!ȳfq[N{؞mpvɞ8#(',f53PG!ȳZ +Yi]#kiR#ABW~j-N˓ibq$q X,k =!h0A4by*B+; PZk8ԹOFJodM)jYǛa CgcDtc +AOy h0%' +>w|&"hH$R.Br,MekԾQ,ʟrcTkCrXG;᧶Ç>7wް,PK! xY̯xA]-PY Aq+NA8 WE(sP55ϡ5lэR%ՅRMH="3E>E +A:-eF8"vWse?XZ( "qBPG;oreG+įU^wVOFJݷn 9ղ0h +A9'ucVJ Hq>[iR3NOB-~e+;ȗo%TNCDG\!jo\3q"3ӱ:/Xg϶,.>q, xa|sP5A>>mgsFFqnayԿy) jf^1B*Pj{IR7^ꟕ7D>Ya:-gJr8h4On7&"w5̳XM ^xe[v>)T{?^!h +sZ#i{Cn A>k&]PbD2?JˤP R!(a:Ԍ!RS$RKA9'!ET<(ALqNBk -b,`5ޘ4f~A}L6yטc,3 *Kk?t3ϝF6 +% ^Pzլ\!hMtA̔vRGj'9g|"DV'Lc%lAuNM % +A?siƕiބVvA ׃ =@a/#-6_],yJ<@&ף.xj ~FX7'lgESF㞕'y"u$sU%hAO:&(UvSWgHI\d~nTʧD:W(l!NH{]Â*QˈL@759GP;bL+6i"N36a{`}0/G_"A}DMZ&,󯛌ρoYtd f%cn2#11h4L<y d3At ARY$fkT?A*ڷo-ARPA45ז-h On'ˡD_FԎ(-5BNJdM|D7{xAVeˢ 7X5V[Ĥ oT-(mWSJ@ 4'$h0%RbAuPE-qk_w $ Õ)d<6 *]G#A|?_k`[:vgpi4 7F1+L:N#s !nF[jY%,W*:K~TjOE>(NS EpeG#OC#Oo?51i"-8َq wIe[n2kXTvR;B'LyGBăNcH2)RN_3\N{4àe ˠj-J+ҌBP+re(GgWq aY A4q#i%mR7f 1ZM,>I ,x i+U7hT (%*X!gnu^g +AAFKI({%."A'8dcQVXfq~3f3L@_ +Aeh0.u,ؚ]y;DK]!t*G-g /R̅,| p +TKu''zF%@ cq;ݵ.Ȇ(!- yUQ8JGP\W8/.F5]fqU(h۩A6IM/7hmMW.(N+RFD @P;JZ&L^EGԁrƛL +.~L9-\[@A̬Ro(%Z +A=p-(yao\P*Ơt~Ƞ=E +t, ӰQBEГfw>B@Z  ΁\:|幆a[v@kt/r8jFÐskA)ɖvRݤw 't*4= P0 "iOEЛ 1UN {[}S5c6AmܰQL]7Zhߎgqv iƏ|pӋ&ߎTkλ2-'`9a4l5,lnL OkLdTUJ@ $"n-3oE\7F]djǡQ C \J@=Q#8.-7IrEg}XZ ouf=f-ϖ؛Cmp=u•n((a\Ü\׈VD.ŹR^*K@cZ[֩xtG}GԴAe&EЩװ&Hc`4SNꭾ)#4 + J4 DװpzqFUirP+4 hWdE=PYK40 tCt;Zu5-ZY"|r"hqj~ز.3aIKH*%MSbȇFz 225#Q6\Z.8'lѤSIsm hi~b|SRjkBnq ֏Vɐ3{'Yvg@v W{n2FԌEgtKt:(R%M]}~<:o' +2g!1sDDE[1 o6t6!-DY:rPWnGT jTAJ@ >}:w @{o> rrCA"!ptEmYi4OM +9k% iVs]hYVAI7\-¹BW9=署9\xȇ4႔ڛ𡝌=0~'> n4F |{kuAlIǮLW:ZӴꌫ=]!|Ah  y2 XfְJEP~3iܙ Tgw{522ʵTK!(axYNn! /U41gZipmaA(닚BTfJN`vdaS: `91ZZugU)1e+%88_QQ=uPGװP?!}g-` +񀳝Vk)\X'o +FMl9AA B +#y*+?&qp1 opQPdps`/p +AAmk KWgb0_}=B (%bC15,ZWEYOPq&BPm72q1Gӱ7- +"Qx_O|.TBn.h+,:BY8SXP5=Q0&Íf@^ykmE}7b>y$u~bvOEPe!nZ&.p"QaKL@wLh +Ao9#,Lng,tfrCi( |~4aY~e߲lfDE' Ze-j t 4JU淡~S (_'QC|50J@ (&P!I|q@- 7{p:kúng,qtA@aAL8At ڐ;ػ@Oy тw#BΧrSI.* PBװ5+J@ @>w֑M!RJ@ 4 +AM3=wn]Ƽvo ۏ+D MtA7>MNK/L[b"B ch +A(Q 8BbpiOsphtFqˆ!;J@ (D&:/'u+qHKF&5gݛxSAڱ,Ŵmy3^3 !MhB|s AsL|!:l;pnbz +AAԄP@\]%K FǿmĒ%}5H*%LSť@䛿yFG7sa`7X,3)9A{BPgc Fȳk-"aZk"b(Uj|J@ $d}%=,Y,X (%D*5X2OFnMFJ@ (%@$ĤĹbRf=PJ&fTDf@K;TkPJ  aNBbB rIP AnoFTj$>A:J@ (%NKմJ@ ߌ hI}v'YjuPJ@ (%g:3pPILno +-ɾ$kPPJ@ (%L@qSJ M@BP3%Wd QJ@ (%@ |2rJ@ $75y*=$> +J@ (%h]:l]zu%-=ԛ{|tG2M='7>J@ (%U-$J@ $yqJ5Ii~Oi<< = FどwR`ϪiPJ@ (%|B@ 4C (!40iI3+r6i8 6+e<]`&0%~ rPg# +cI 5}`b Nq9KmUJ~SJ@ (%@4^zPJ4Ria-P@=eTvGu P.ǁj #PyxΈfm|e;ՂN(z%bISϥx0P{у]SSCV@A>7PGe +jǣf7Jz\nS8ؔ*Q? AShΫ vAi)1єhWCsTg\k}qs;Iȳn e>Kw*Ŵ'ɔPD:̚hbM1Γ>e$ >O3l| b}vT@A]> BA7U8 dmԽ{5ap u8!߅<076H8ywؿv*1;D1R┛V 2i3{)4un80>*,`h.d)j ֥WWJ@ (#LbIkf:xةr& ƫTx x!׃’VcSq1ZPµ8 WJ QIWĬ49CAp5 Q!BEГ(}p;NwƑ13]!hI>A;ZWXN+}AxU >M!J0vr^Cp NƹA<Ǡt6BːYO*[6ٲۖ#ir6S%J@ (%oLp@wf_ib[mODx9Cg6-a 8&A⬴J@ (]DrsrNi;jP Li~ +pQŕ8{@g- V,,1Ng<@!Z=sA{!O06cU J3F ATs唀Hm(?lQ>RVJJI#!-Yk? Au9TL'Q=Kq/Nt + Vby&c^o!f!X 1'2+ +AƊ |ٮE8'z`KcqATT<N5ĖȎkt&MȵRJ@ (%;ˁ/LEF!r-Q5+sˡس+!C0>4d=BY /uSJş-q +lvR/W3\HɾleõXfuTi(b0#p!=q+a[;llrMk3?ҺqsscM:1jt(("1 fqK2ufcs.HTO qJϠETZ}db*W,v@`PNˍR>Ldp +VU (%G`IU? &)|7)%Y`>@l?ZΟZ7/]- aㅱ*8zjAj;z&%@3DȋfDkvTz{+r<(m7z8l)?A~/,] Cqz[:`}[7Y< Q=C.0&ò`ybL%91j4D(bg}t|E*krP8{\\|WFLA| 5QGE,{[d˅l/7H`|\'O (%` X | } ^CK!C-h.>AtL19ArY%oc"(;=! YRClL-PFTMNR5JʆJIoV$ t A[v@60ꗡv>mQ2 (\xg!vw֎kEFb͗QPgLFf1/jg44ZJ;? At4۔8Pq;Dža4Wč(}寣j6j]!ѭm~mnː#rfNmT pkZ;I_ -PJ@ 2 lz} ~Cixr (c#YΟ1Xghd%b.3akVnXp) pPwTW/(%ΐIR?$X:əD;((yg!.<!,S +Am`1 +#M6iE +QS"ĞEPP +BL|os!b;3]qkti+]p>kc-6ieSXvȁ6rP.uRKR?k)PJ q8 ? 5b+Uki9Ԃ'}e`A&Rmt|~6>ucmeRzy%,i8į#uN6?$# +/Wb0^~!;XKhQ*(Ɲ|Կ!uϻ2գj)7:ɥB9#'@ >P*P]b_(Eb,N±~8A? Ag*X3&0դs["5=!tl|XWG;L&(>Q+Qw:(h': A)CƑWnN(QJ@ (%Ndӱ+ml]1ˡV-bD ƎM0+-]&\l1X&3z[(x.w+/cY,%=vE=~5?iAjK@P!O~Ծ,5fT}FgVet!YV*XEzA@;62 +A4u:5Gc/EPEЗ!7-(dX-3dtwh N) 󳱌F8H\O9ܜTUsQȮv aّ-x/kݤ R?Cߕe +z}%he8ÙؓalNA JHK!>b  A kGNkF o@ܰzV&p6 +\ .X {>av_Y19]q'o/ +:*]!HUVn|PJT+8}/vRzL*IiM.ʐaٖ^>T*TEZ`GY6-J&BUZ  6+U9,37.4B6;l yyB4#yA\!ȳ=Lg\腋qyQ<gٕrɮ–c;>W>ߝ=]޻=ߖ]-{sٮ˷a 2()3@%$D93_OO!@Tӭ.Ttӝ 'T<©6Z&-"@tr! @$Pfk78Yqgy~5L zH vP +H$Gc~ bQ,D#9 +IX8" kMYLP7oN! (A)Q69CD[Vm>5OTGè4^'t~vե"hK{h="PA72=pт3NS~lʣ@8;le8;򀙵tm< wu&lu^+CQwPQI׭774n3477su#ݰR`zH/M U:6h>Dn?KJ H |N 7~O\Q85x$wݺoj(qAxg뮸₋& bSbtJGnQ8?"++񅌯k^,P=Zd˾J <[N8aXqX5X˯8ŪEap, GJ(c?za;p: n E@DPo'*$ AQjA:*"4^УiqeXιsiܴͩq*Z0v44(CetA{A?*A2?<+;E;qCq޵&l$x!"(x"hzvE:CVC>c1휉2)J0SEP9W#ZMGb7{o_ȒP YuqGd .Ϸ|N`)4J*hk| +:U#nVm((4If_"Pj{;ܐL*2g)a:%X7b),i5q6|5}ia7!Я 7^wb} `A8+F[j q؞c[,DcSZ5h]V`Y |`Xވ}_ot0A(B& " DՈ {*&Q,DU#㇇xɩaCQDP"N 3636m5533ιPr)߇ +*1T=gS8"!ɏ=G%q1`Vb!O|Np$:vCVAF23Xk]<@04Ơe4S@@irsZRP̕Ph a{'Jߍr_Ζki|r,-c~Kjy$$cy(քq>ۣ;YXN%\"'B.&|t NFp P p*o,q\ZU}Ÿ@C@DP/DtMqװԴQ4Ce1zDw|l9^mZk"hsFƕŨjn? oǠ(OcPN c;O֋f^85L.xEPQ\,:]MW+ƺeN kM[[YBrTegEcP; ; B@.p8ȵ[l+bb|f(,b&:=:a3Ypkr^'F/Q(4F瀖6>b`z qߌt jrf~h_8l"Qja@oQ;f-o DŽuXdc}(cp1#8uY]6rXG=1G#4q *=r+2q PPƣm$"hv߲@OxhDFՈ A;h~R;29\8>-3eɴB8e]do];B?GL.Dr,2q-9 ȋW_wHdG + 8}͊&6g_~&3RAi{! @! "E c[DW}'nTgTOoP* ]vγ{~榍U"hfq d 7 ~rG"y~":RXMWZE"pDv~JDϹFPqj5"mQ'5$5GY!`!E [wuVpqhag# +7S\NqT bX8jEf#v;^(?`4Ek 6djnqA }<hyZ PBc-P@\Ww/K Xl"wzc8`(N|2"zq; x<ǣ8cq?wP0`d/N 72csnz,/3(! AFK< dE1꒨Z JE&lUzvP1MmwQEvD -"HAqq,=?Hpz~ߑtʿ)Bo븺 Z0W"#5lJBQĩa3"iyKa~ٍr<)#X*MՓQiO aKG,v@X4D&Aj%g,,'kXhۛ nSF:?SN3Ou +%8dMF4 NYl/C?S(Zu4ڦP+]DЀ:scYeP;mѲ=Pk*#`;{a?qh%.D׵t^!X!п ,mZiQ U-_<|7p5#.^8?X?pAc~(uZ|^&7+.n?^]&Asz[DcNN&ӍH3apWq=nFp(D,cPXǢ&UxϯXhޡ(^0^/ +D?r-#F3497)"諈d! @̵%v SX4w +wCU=xrab,ۜ;5Q"hʹ}G]5A,D#"oa7tNA;u5"!$S|uSFo爠UM-;-mt7㽨ПבT@5y +,4DڽCdBH=m:-0 + (ƛp "WdnZoR#usEP԰;,N VvXQsjͱ:,8{NAĿ&5LAjװj 1[Q'3~qs/].m,StJ9TI|e0XEfka0ɯB@Q@`_ +XMDXT@YDe(_A\eXZSN6|3p:z]_<`TaV/z͜ wJz;ZoF b~5/)*B P Fmky 5̥)T9ʣTڛzz2 6*@WTIT`V hN^X$԰ +"J1԰Z԰C錙2tB`*W*c)S+JWD{INdžD0hh3k`_<(h xGP<2AQҵzqZ0Ae6h.:)|aFpˇa\F?4TMju&lBAY&0Q*o4Up u'm2ڦj]'%ǂACs(ZzTPlD!pد= $Bԫ{đ nՖLM~"U'RE&ȅyf[s[EPAA )zN AE3LM 毖$ .ZO G0AL.|+7p\1$Hf}ViA/8"(%( XSSfSnΙ誉違7TLè-QsRQBWw,\#,jׂ^i1(Iīh<ã ziu*.e;G9EF[z#9cǭjl{7uahDЬ?>G@6E!CxN c#[Qz/yÁbJѰTاEpM4BSFC[Zق:#*xNjm%G=,B@tA݆{f:LUmՖD#FRYph5"~d6Mmja䈠"H"rލYUA8ݡN"ܳE1?,kGxBDL97! +@1U ˌk[=F:jFr;&zhTCAJ-dAD#7_"#}AvA:yaS!# ťƖq#׌_;˸O[A!h ŝ' !  ~&?os޹֊V@-Ü_|6;[ V@ +#.6M8] {'{%-j_]Q~id! 1"z gt/D>SI1f3Ao_7]662644m2h"P?WAUP: + xkŢA,ƹ)h^YqALo:bģˍ`k Ux#<"UTFfk +k m[ v`l#噩J/<5F5'RT/ F?;! @#E;׋^{*l:KAP9 +ejzvA' ):ŤSZgu_S͑`y/d%3r-Zb'J"io@G! ~ 8-EW{{ȮFJ\"'pp8 )b4ʿS/u׏\OJ$WV (k֪$"諈d! @Oԓ?D}6N1TOmSO*4Sޞ]ַѷ7B&4Bb8RXx<V,S" jhpTA6CA6"#>N)Ef1qC /UUSC5id[D3XG^! >AEvh`Km6~ø0qmnf8Svqb['t)Sf 'eAKXAFPƄ.x`Kw@`ع6! #sj&J:=Zh4T @ʟ)ʟ+ʿRPT(q0S8:&ksĸP&OԙQa pZ[~uD}L I"zgY!MTMaTDE>ԃ=h;ChمMhZMqX8<`55,=uG]; rqAAx7t,Fv H ctWA,,P1Z_5I_ꛗi鄞.)kY衕^zRQS(&m +Qg|Ǣ|ab, {*ڦ kF"48\7\3A @@Fk<7k|G!Wc{ɤ"hی8h9=9*Q: l^b')@h@"jNS2(3ESE?!Hagh!:ĵ9((J}Ǹt(6_37p'|+Ձ>*6ɠCWNYZϟ7 (2eEB! @'#LSꔉWieڸ@P `a?,X,R_Y.hfFH46>(u+a|ѯUD2B1_["+6Z)$^Q 7ttMaSQ=Uq(īp< Ž` +5_{a*IoÁOyaZ0Za:}^ ZUtSA\,A0U&l,n*ږVSt.mGLUEP}P> oP䇇\5;2\q' D-%O +P-B>U|\Ⲗ/#~APu8q ɸ8T;?Gjʃo/j GW*"@!cD.,h GQ[5Q`zI驕Y覙2M܁~h݁ hJEc2n +jF""ax:p|E|(:G8? +LҢ8ŃÁ:0:ꦖb~@YZuv" )  +:AFLYʷ R5ytFOL ')E`:TBCj#>~xn(LjD*j A@|`i%9 8( wA+|4\qɭќ<ϝP*O4x%ma~a n(Ƌ@WSsff6 -J:P\D ɉCw|K è%꽨ʕJܙ]V:G6C +4-B,MF(TƢbJ9wM\8dnp7f/25@l"|H sqU#qIQ: Qj ia0K2e#0#9QjܩXDȪ󿿟F>?7jD4GaQ懷P䊧&5"ǀzUj*Nth ^s] ! ~<oZ&~rG&vA'j05'>Yρm>lI-m lQ]&mGq:ڬʑ(U޸#^hWVR\PW(4B@&I ԍg%4S?׮DMtÉ2謑p7.}+֢e*2O$TDe*{sw/\Yn8䊝 .q4 s~o ia[gqW\;nR]3ACTTrA7 0NBMtZOW tHY9;k!XO,G|#p؇5 >;7YFuH8 h`|`g@(wO߸x\! Tf'2˂wDM(Мl` UGB%m1ږAc\~20uQ='bKQax+nTԮ]D+_B`p"~?4jg*"7zJ要2 t֠]zڤQ2sMjC?<ll;c˥~Z +pjDsX (fĝX-2k7a-8,wy!7b]-̘SÖ:D^Ag<=gygVJVD|'n+hBxՔZ}ylHkg >?P^{}"kB@JG3c0TG> c4S89hY$$u ZCӱ)hJFCvlMCx x4}"JQ9XuA,95llDv.vu` {Y! ?)kn¹8Ӏ'nxPATpɨ٨hdUgP +t5+Qx KQf>gt<1(yWa'.w Y? o9Ϲ{g(sQXZqqoa­ƣe$bQ!(+O<}Ҡ,pZ;5V h4t6?Z[ ʿrl=RXi/FU"edB &@uA-j9҂M=0$QdB@! Vw>}s(͉ǁ[\φe4Gdʦ|&b/A2T-G +T* ]U(_(Mûxj^(cq;9Ð v9"wn=efB@OGkEu~!WcRaD~G0T)x5x1^/Û4J]ūux/Vr89`¸4Wfm8(B@?0 pOqv9#6H8.G}D~B@! zπnx#k(rcq{' MeFx5J>B@! On7\qșspq{)ns*Y5Pn­Td/G2d&dMōq!xf T{h8ڟlC|j~GaJ7MY_> GB@!0`<hdB$\U $d/ᰟThî!;Y+pu92!#YdGx9;-wύH Bph_^kL +! @!pF r k +2g!c2 # }V@]Y帶YIȝ3Q0Il~D~B@t///{D֋jۋeB@! (ûPDm<&dJFA9+XTٹ; M k%@"?'-rc@! B+]񣇎?z]LF! |N|~dm\mB@! wC=pBles\m0B@! ":"2B':.[-B@& F'_B@5~kk+-:-e&B@! 8d9pl@g@r   L)*`3G#uB7(hE[㺿Hf.['BdŲB@ 4@#0Wlw+T`|XkTD M0X +c{M8b XqQC& QG($(<Wf&Jb9עa3ZwiN蚙g$),B@|7F~7:B/ԗN\c̤5 ;_s<"\q,ggt.#vخqFX +qu .Ä XbJ 6a'{\ŝëx3sQۨonhg1Y+eY龕^YʽgPRB@^' ^B@A@DЏw)ﾳ/&D4@}#Q1 %CP'x0!8e!Wt!g2 j=„8}ЌV/^鋃8 ! ÍHq#P;M@A@ɠՠbzo?%`Y! @7@voWgh:̶-@,4LGdQ(J<ŕ` qvAq' Npڀ #ʄf,b; Jolǎ`a871ȉG<fd,}js6K^k^tmӣz@U#e7v7! ܐ>bVCCFZ<17DVS81Bn[D=O?V!ЋD"X\{w.bhsm1v.*gTO1x8bq+ +Cp"'pr0Nyκ)M9b;ج5Os+# }6;q* +J푸?L04.4p\ojxثOT@O T3&N5! A'8j!OŞ HŁ8<OMПh2٘=)ot˙w3MULMshhWY'`OY ! @ԭ8ݷGڜ֟} Z6a-ꖣ& vDk13a89qgp-d ]k.8 c^& LXc ++=10be 6P(EfnBW連*:WT^ӫwqs+Cz݉czP,9D:6N! @_#݈Mfuj/` EpDlMm~$þCSNN30gʹR\*'k,Kl} -7g:):>2peB@^" "buy,]4 }a}>4oC:ԧr)Jz>^ix0 pc)g#p&ȳ.%\vE32 .Р\5ȀF1a [XoJ7,o$cE օ`0#8ˉof*+S]ymܰǯp`鐶0CGѳIv&UΣ$4 JBB@M;K.6b1 }0 C0c0? G#yեP2]DpY! &0U)NnHj_ Ě hGH,&OvZ09eKt536ءxPN*/*Y]+ Kmk̴H7pЀ8dH9 vl~݇vnrnT`\v-PU[QoVh%<ܟ;S;W }\$.!}.sOo\@MA ÂnX4/,|$#u(ևc[$h<Ύ1IS/0ϳOqJY[ͿaopӑЖ3m"Y1t{=OϧQ\JD.!*B@M`*Ј436X;;`wvEbgVN +u+'O1-j^8ut+fzU7)߃i⟫*Y)Յ6恲2({ZSC@DPrC{ڝʽ7=n; gU=p P 6ZLœ%(\s799p.q)WBk>D7qK 4KFqC1/߂]VluOEX>18S#>Yp{2O?a|3\6۵"ɽrWZ6heˌ tw=FsB@D{+,PӴFS]4U8zETB7({-ӽ岥ᜥAKn[\^m0Xmz`"}<(:qq .%J 2#p-׃냛;&C nD jgb;vzby#Y Ar(R#> +/G{+9H?Mhht|%^U+}j6mnt,lr=Tvu}3X~|. ZWY]cqT5M􌻮GQA +~ك3=j/Wv+?j-vK/띟r*\aؐ9squ&ҧ }<.BFn[pЦ#-g.%سF`:=GoSr5%蔔]!ugޛE~JRInys+unRRTr_ܺysww4M4 2[X~ +-v/#0;v>}l,l_t "t$~oi'mu7WMg_LO*)F2 2鲞 izm'%MǸDTu +zroTuhD4+bbrhk*x.COx`;0e5Zmh'sUOA?r|:L)툖Ɇ|<^9ytz6wW:=u5U1x:LR9Bi _#+`FoHR&P`\qKL[au+<0w zB%aҖY}:JQ(S>!{1 :JlɾTbw bbYGo(1Y"T)o\-wIYF&]i~6YoY7Oҕct.NaBGƏxj-柛M=66m<쿢 8ߚt;Cc<eB$_T uvhB9^Uzh^m9IB"C!Gi+B`KQ[q'qEnysII2T2Buml*'LSճ_ݷZF"6HK:y.)ͥv0#XR9^eACL1e)+LasNsł<_b, I"#e-Xê8E)Q>%4K5,]&g-"Irn f=q&1Fq2zn[FNa?_O-/TV$#0C A+f oF.҅4:{N$ӡ]펠wcJRv=\Bo ?ZLio :DR*RZt #0_Uh4Bƌ0eYs`Xsƺ<bK &Ҿ(iO#֠kQdO61I,/ Tbu$O_ř1i)?_(+`fF#fb6Sy|.D +Niw,m״m46/;.V=7Sh5rȝ=7Uהݗ ;6Hꏢ0Bz^uExmވ.D +^Jx9 +x6u5.Z"'t[ÒqʅexHK +^Jke"o  T1LQϜlxc]Gn<5/ - CN'㔦QʯI0#bdDM%k &͵{uYolP  &+!|x:B27)!-İ(ˉ{D}'-hQbD,O+D}Yuh"[>$WZ>7aoU0'xj`36@JMWk'ϞgP +}Li{fk&f B}=n]Wv_Vt7hMo:)?U&AFyQ#zqG#mj/Zx#Zr)!CK8c[G#R]5/_Cɳ 0yg?Z9DTGLSSg,g/g/u\}躑)U9vEBJޣ#0W%SSԚƔ=f% +Gh04aX&th&Y ,FMEKGB8yk(+>jA7%>EID}j#=3-U5E+zlDs AS~*GV`ˏbF; HD6M"/})̦75:{_CghIڝ8T*A/lBP,dܝ o=ߔW\\PTuhpF=^YU*tSazGBP 8kaHqιsW۾|:Th4VƩFU'̴B% ۥ;9y؋VW+ҩCt')ͦKe1#9džxl +Kދ}q3<0'!(?Q1P8;8(FpHRd&V2+_33)!fS~.,9AL/ ATbUъXPd)s> g"smZ0 /AqgmG)͇t6]ʤ3u:x]6;oŬ +]i[w_ٯlBPpۦ}7TW:.5֟W'0*P{hAڶvk=^shV' _+P,!^9X5Rml'pYw8GO + 2 +7)#Qʁ8cd:&$Zo<7ZZiO(3qt5Ӕޥ4[FQ2䛡Mwn¤/0(`#4V'u@A&^omB6b+՛53liAD#%&׈9:NTDu&N|(LGI{cSB-;#bm7n F2e8R֨TGr:MGoс+ :ّTP8_n7[oCˑ{黩꾦츬h=oД_RR}+ODATD2 5A^:I;eڔh7@E}"b."J""IDMqK!  OI䁯4_4PV,o 5lWF*?)Uc'Lf/Z.^Yɴ_ĕ Bm t$EӍ8l+V9#0? +{P57H&19Xdž ǖs ГCx!R{tnxd2N:Ղ^#aXQCMDy(xŝh(Hn,Z#e?'>Z0 0!Dl>QL5TS@Wc:ަWi9MwfgkTP<_a?Wb;o=x@YIwq 3Tc-!< +A0ANxi&+2G +Ѓ3DE"QHċDUD'"FDgD"."G:XD,DNDne8K6{dc}M - AZ!h,.ҍ؟jfF!P@>T8 ( b c| #):'hг[Ns[, Y/y= ψ}bx^ )q1둲R>B*CX 9Na;Ĭ#`B.34ՠBהꄠ;tN:љ~p%j)dwmb\3\lۦ=7TWm 4T$OA:-!Hݼj6Cu-PKDG"QH!B>G9#͕{+-U^_PU%PM3{Ipj%Ӷہtc#^|/5 +X4noR@> hS#7Ű%&0EADrXYo:ƽhrA/jCK'c8fNb4ӰqW9@/^Խl)BF$(BG9(<,Y[8(9 "dq8'#l"NdgceGcdǢ/EH%E/ w2Dw|u!֙Ý0&/Mtc7ҍR\@Щ:z\}4ݧ6:>\XP4ȳyh9~w{OOIgqyEs!E&Y"I +Euj[FGP-&hU5A.F+\A1 [pE4H ^g!H+e PÞpŻ8aqiӹ ꥫVl7i-q5Ջ1_)?B#b#>#O @&(P` A6J 1?b`#g Zi$#t8ᨉCatEI b&0}r #sYw^)Nn)B[rIhx#ArHp@oH'bb]q8iu$N'DaB9MΈDDCVgW`K]m^9FQ0!hYjJ)/r1˧=:p_xsbZ끕冠ZJv6yVtO+ft7j9-oL%KuA[k=P{4 +aB--J$^KT-s*=d +B-qGmo8-]nD"QHT_ ʻ#h0Q9Λ/_\iClZDk]i;c^tşn9Y?:pBSRMp[VȵEh[ a40SX=eޖ<=Z.B?Q[4M'~=+ظ}As hn~g +SP ;H5&:M"y[= ^FG}"QH(UD"h(əpIJw={*/c5 A;j|ưgJdF`B7Gu+d7 *Pt.ҹ:>黫ݩɫm+1/Cj}\lJN=5z]U-e5E%yYTqᶲ [j|/mh=xe $QVj  + uB +LRptB3.r7<^\KH&RKǍT\QofY64ϖims}nt“Śa.3_phsH A8,G'LqZsVl낇xO6= pҵӲtF\FFI{tZu:Je>7t*ih\k],ea=[Cxq;q4N!GhK d'_DhWEhR$Bw4ZbPhq$]  ҃r7t92L=͖ߣXF`gԘdne~ٺXi_6+_Pa t.sEt*С,>Cwi;mZ[jsːZ*2y^=]d6o:HQm yeY9TiQI/B5L,Al $QT5C^B +plx;ui\$[WZ/xQ^,Wjzo/ͷmi}D'] A7:/Md H9&C!R8jD5ppWq Y~x.Q\w@D|('UL63L5f4_M-i^:lBה_++hZ3$z}p$kؼ,hzT-Y?+B$hAID"E߉4DZy0̆c$}hDC$"0"5Ky_d{ ifj͌#3 k1!3GԼݦNJz ].t:?MM͞4mkbV_$-3&όGr(tBA9YSTAQ\ujt1`[BfhR9Y,+18HBt_L\ 5R [$i'\vMw䉷GZ'ɻc#ɣsBOh }iG;A':I7|<" u:p !1Hp[7n+Ƹdalq 'ܐ手!xb>R&J-댕P RNś,%m$5,e+z׆evɞZә?qկCW>/cS{dC\䤛첷^?n"; '>E;!8bG5!$ZD"61aJH&M +6!dz80ax#!*!AH Do*υ[{u+-8wAXsF$/1!O'ټ݆uPB7J](3&H鿴szׇZVW꼗muBjh4O1cAMYSz %5\y4a.s@-'E/d&=4A+IQ$5VRddk;\{'boi~5X.{+;(="[J1X?MSX;G!p:n!C (<"_|9+@,]kdHu1W$xNBN$rII2NQOaD#6GmI;zٞuN̙6^{:B5? +zϏ&12=]o\A3QvW枴~}y#wy=O/??["^} 4G R%:mmr HAc3X D8 $q|S.hÄkWg ]iV)Ff~yB_"J"Bv6/o !}|3lUt9}J'aUͻ oS7D6{-W9/.Z̖&K F뿻'}u]~k9AAEiCjP:{۠ fpK +A/uBP qxBrg[*\19sDȧ9 \I֋YTmbA 8Zp +" zM$"00 4|p q'iu&ܑ[_&+`~dT{h|`Ȅπ3aΰ 5FZIKlOGo˚g7{OntYo?AU.Kev%s&SBP}{7qgД#>(TusA;h0 QuC^gx$G7qgpB,.wX=҅ջE&)TyK$A0Ptbd󨔞tCzGTgFM,7]o7gjsfL_-mZ%x%E UR=(Uy6ȱ]Gd;z8+>b DoyĦ`is+5&a!X+kB":KXHߧiNltTj9_d2]$/c wГKh;h8Dy0Hn׹Cj-P)j#TQJ1ʁb x` +\S N qa-r.Hz%%8n-4}zŐ6*BI[;]T s vot.C(5orco +Bڭ~N{C oPMf\ʭP \qW}pHCB$NDsgtwE1.!UDHv'wy8uϕ+^7}h/-˛Հ|Q:躯PRV?3yYj;K{JTMKTc3bJZt $=iѻ$bB !}/AqX \ 0胷x2Hr[%2X΄/j][vl1A ;z3z˄π3a3K+t+t1K'ӑ tfOFȵ +疋{f*'> ǻ轆h?chL@]"P*oсQgZkXJ=pL+P,

76ǁE}@YJ8o$^Pz)L+ +ufMYomCz|7M/>7\ek%z%(ƦMLp G ]"m1!%& J1OuC#Ql6j5RpSؾ߾fY#| 2L h;6#BP͐҇RZ* R:OkQHx~_1Ű-zfgm8rGTG^C L1„&4ĔUbУF%Zmh9 Op |.I$HR<1F#bV@,_:bBrM5=ywrf@,N:CW<|6p #$ 䚴X[U{ͼk޺+(&y禳/嫥kRv [ +ABG@F`%~ FHR`R+X⊰Džuҿ0@Yq# l6c.Xߞ{ ;w*(}D7r:N_p +?Zi~}Lm q:z΢$ڒ| Ѩ AQ:!k/M`:#H&W%z(q@H-u.qTH01C'-pq/nX>٣DWn7}MyP9&$͕j.U*`~70@[^徨 T +q{[ YcS6{0eiL` ݋76hCj \wϏ("9.9%IK"T"@$"bSAkٟ=3vo!46^?:Kiw5;'9lL ){ڞm,my%o۷~~n2- +ZtD,xiCF ÄAt[ ak,Z-!( c nrBQR3<0E-y;W|Tb~F "})lv#l:yFɧ4ݤK7:uw7Fmt{;T[.\zfx|2K5\tZ1 +B _BTkza1PwL(e)[[R\ +$+1ENq5m\H@sŨ;=MGp^hG(עST|&0tU,bQԅ)X&1kY+Ŭf՘3Ì-0k7hs@3Wƽ +|\vf$=\r&Rz"FO%bz'yĦW&A<{O>7~!`z՜P0ZL_ҡ俓@rk0?o̢p߫:ޱwxF޹w]{x~i&x^Pj\b (ktIf8LH8h.@P4X&1lwV6C J)))@8Mf]n{lٌ`v$|>|;(ަKt"]D4G!5>pC~T +сQoMTâ +=m|aSXg@Vm\NqDX+q@=8mt5n Qa&{qõ ݠxN@,]?օ oB`)=>kJFM" ꅿG;Ř'\1X5bsFaNg;W>3 IV$#Lz)R`ZG`ZD;H]wޭVMF:un-GY{ռ_=>G-yIxXP _̗->~ $LA;f1&j 娒ᡰXx r;fnKĐ#}Bݥť?ߧKMj?G7t6NDС Ker|dRrsw3$9|HE23mhG6OE)* C m@Ou*}Wb!Yr@J\T +U(0EF%0KXf .wf`w2KXt +Z."Ρ jO1q='h8dF‚;O6Gǒ1IG!gwRq\GR'){,*!M?AdDqȞt$Vm_N[;#^}ƎNϦ.f<xDhӇ % mW(6HMJzxv޻*>C`wN}{zM'K f s s2,Ȁ"49M4qЄa]+N ʪ>V!W 1hW O [~/v7#0 %O'?s'i;t_ˮn6oOw#.Y/:FifrZjrDTx>3:G{]Q um\%V(2G PlxQa?@I GL%)pZ+ +d)@*ރ>v?NXZbB䳃lRf1uK<}wБ曨@UT"ϡ4:R}<8LE`.XŢoa.wŌ{/zq]\[R+yW(Tz7B%+=MdGaQ${QJCj>jZ2k۾KGM &ӏe9/+7v맰~Xzw WaC5kx$l:'-A?`y~so>^z坏-bv6 cESt֖Y[i۳٨ڨUUz˅#5̜Iƀ0k8qj-QfB7OK#IB%%Q+DBQ!fWbv5Nx'x:7w5`Ncb_ZK{4K".ja)4ņŖs(Z&>tZPp==y7TbK,? >S &s3$d_3x˩XNADj/{bNCMarhS +&y]aKƞbFH#ozd%,3v%,i:lLh!mm6H׫5+Xś4LxF0 Nëڬu +TJQʡ%_s)8p8!R` %HC#\2V`@c&}jb %'ùwJrǨ}h֫踄s?&a2qBC,cOel>:->Oۓ| qAQnz";EHADJ%fMbu>Յ#.mĭM<3so#ߍ"oZEiWfVF6=˼4:{o6>c2wP`+zŻ +1;xbB{>)GJɈh:/`FusXƻkcD9ڕBGwC +Tog[[\۰1LcDFtM߁w߀vWZiʰQXu,^|*f0uƀ/Nkix(Gr9x%҅@: q@@AbJ,N{Xj)ް |^-Պ^' ( +mD=4AMt\Cw:/` #)Hl< bp,`9 ?&0 zA՛*'Q[t"8NE1>NL5by o&.]qIH3kГV7t`c0?x|0{ }|p괠j +ޯjݛxRbB{zϑ Aâk9'b"a?Qw|p@5^RqfFe|3%:ްcEMHmh h h.6~ X>Ol Bc#{PBE5=)n @\/@4 .C|8DTݝpTȖM7X7Zm2X3\PR`MAkT\[BT1%K;;}WOcf`> h,G`9 t0&0 /zr\/W +¹GQX.NA<(41L,~m_N^vO\OzN_c-wo\Ů0 wENV,NχuQm|+> +A強Vwi(TlZhiG3WPs +z/$)H@b1A_~Xertu3}H;9u|9/ 4UdȻN|N +iZx*ޫ]K5X6=*Oٚk^tfuEq=1xnvA-ڴ¶b@Y\YI[+>0.%j_\ڥ0v[, z4XҵaUt/Q%A)̈H +-lӭ}'0٢} ]џ.fzJ㾥2iqx7yoīxyE~M)^ud6_et*a> X:ee61A !?`_pу^1t%uxHsaxZT&1*lƠ' AAQ8r-摏F)1X+q{D]L,U3"h3sW햲lמPފ3VFhiAf/yYsyޭwm坛xǗ%vu +dYgVxdX_󀫿+ E/1Qh D'*Ql'#I@_f4=d=aKžwӻĆ3cOl+AkX<_2g;}%VUEfLO=70s=X8#X:ea>:,o+7-DcJ4߈E'D"QHT"=D7EK"i(Y$:".D"CP$ &bEhdU,< ZeBG)'P`@k50x.ޮԶȺYU#EuA},_Gy&QU"bQ*oNȷA7/E_aF9~i'k`P1 {fH?xjz6:ZC#cˢ1>d}yvTe%fŪBgҎ|1C&Fafh-b2$Ď>M%%F + ƴ$dn=|@»55M9U%-Ez\Wzs! 71~31?$C#Fbg!Hȅ=O ;xj<B*w%Lz6\v<\~8J$gps(+!l4tu=fjwI^lp$ZN 5RB wqRW~̖$d$ܙ7]]]4M. , uCAd\@EEEQDV7YewOuZo.u>Ϲw]TrpA}w\cnmn}'mm /Q + iCm5DSs1HLAn"PTq3O_7'0".F@%oG\/wOoYqc.saɨ'ON$oEzz996TDynxGeP*wݣ˷-╵.}7cN&FI{)CEG>7 / +MvE,<>ԂKo?s֛Lr cY2S_9e^-sYSLOo/~d4fr2! PSR _ᚯ4 M6-'ݡ1{ƕoXiu.|aK羙p*±胉g&ڙxlk Ak}W$z,It^sUDцDlvn7G_=eAxs3:(rƂ [ܧSu:]?~kZ4P5c/IL8{81zOlK ۘx`mw{&.I_h:Q.߸0ι{?fWc&Nwj|ޙ1ѳtvXqYهt~St:xvwբd Ҍ5e?g CS=4#_ _9<yGk?G@ BIn''C  + 7W2_Ov$wewjŎW6ϸ~5]Z1’)N&&M);nJ<>qĽ+ݿMt\h*Q6QaaO -n|[?djKy''=> }w DottՖn֗~zofާ5qC<7롼y}=4o#-h#:Ne>"J% 4SR;^'&A  + 7W2_Ov^dw|7bJ7}eK߽vqٴ󋧜NL:xPbij;İ O_, +4neDѺ/l_wFic/睰q AOi:mA`S}t#]tt֮Z]z^Ռ2PiAk=D胠tv*&zX) M/œMɹ @.t҉asԯ'GKTwr;Z7+Squo]^=.,v61DÉ L<-1bcuɖѽW$6aywfQ~~Mm{ɮY{g4=~drc'z>z9 ȩa:=DglutҡA[;imW-/{蓞Ko_ Єz>;H'w~Gtq$J+4S<^'Al  + 6W2_OW^q^sgoWlSy7..}WN%^>Sb̞3;mI ݐ6DNKmW$Xdg6~\;fd􂃓G_?66ɑ:aK,zXL?G:YN[;hmG-/j^7CoܣɽBo=Gu[/_b~cRRxwJÛg+rdlr. Aab~Նq1ftr +Ns^wtfW||e߹+f^\%O3Xbҡĸ}vћO_gE˒De+5_Ym|x묢3V G=N<#:Ggz:EG:&ˁ~jֶӲuֻ]z7]{htOK3{kuzi=:_'JW_+W 1x508j8k3WB:T9fz*qSoݮ*}PiNWWuis_;)G~JޛxrgbĐJtYh$bU5[q54hMvN+3)~`|cGGF?#4ȉu Stv:V?Rk[ki[}NuЬY/vs]B7]i~7-]cwI +ޫ7V} +;.#G  +61s%Cc]t;;jEAWly|ì2zWL?d/O?u Ğİ$}ĝK͗|Y?.Z~ +6ψo y=dtlۮr:A`+h=%|V>h7vAܥFG,vvй@i4xFJOAx޹c Aat~Նq}\NHrbeYnuvƷJϸbȯv~ٔ3K^:xܱs#oK۔>iMDɒ.hb^ +͎oeZcwuh9j]}Yf՞V]M7ӗ%m[jR+n[kjknmf@ tu2+os}?AP_4 +!M!J_t^qfoWnYҵӯzqMξLq&/u*V?_˞s;c??uOD_o:KsdhD}?Fǵe ֈ"[{kswmꬍ6ך@󔰴'9QĨ걨KsHB2J_.5 O覌#@n +sY {,DNugtGǺcܺ'*?VdK^+GiSZ;\? 1;Cuw VkY4O%M&HHIOH/qQ\r-@ j(i^V}jr(<6wv+T.\do^ꞳXOWu^^Z2FKъG!Okљ;ku}FZ}Y iޗfHGp"ƽ xKLE(ic {A7q6F+fslvGn]?wvuKڟ]g~zmmF7|wcY- P-}Y^Di^\-i4ZzT'$@9~p܎ @ 55/@= +F19T\Ýt]+,_ҺlQ˟8?%4;4 +wψ|]['kx5Oh#Z1XKՒnZAO|sl Q\kFLS+ !ɔg%̚K(Ɛ@ rn9]VVL#'q#p+^6E ++Wy<@'gL/k)l#Z?XknZ!Q/ +QމhzF<@_)n\Bus@ jHm^ ]V}IrQ~(t+ Be~h*GWf:;Y^Ա1:4RОG~m =}Gn顧#_Ksה3edd_2i6B9.PNF ʽ9C/O!'V͗+7[n*_Q.?#tAOՑQZDEmy&5wjj +pܪGK,y]&S9%mS֣N=b桸rs9kYPg1rs9[NU1@J{+@ ʝLC-[POo0慲L{'&AY ynsrG ,EL1Ni\^=;;F ;7Mxn)޹'2g@=s悑dwJ&cdLƆ @Ȃaޢ #zbN0M;i dYPOeޢs^;[Lj +pk@rJ,( 2SY} +@x};w@ M@~zyrlf֙@ۧy @2V,(coss9:pA %Õ @ go3j:L& pd0  bx %O3kg&1 S9ϩdoGv GH@us599ބG7 YnQLTbRfh)w]lɦ\@2GSdɂ2aC /-٩ R0Ii4MzK@gAȂv~A;d\9˂ZFUsetr2 'pMsͷ6I Tljosm9~@!)FJ/HWyBfS/<]r 膴 wP( ׇ9Hfc ^,(<@x[6Z/Oҽ#ҳXeɚYς I :B],Rii,TN @RaNmS[S ~7xA !oޓ׷(rnk&I!ȂI}dkĬ_YT]eA/Ȳ 5b:8A5-wPv+g аE@5C_]m|N#~mkWޓDs%-SSZj/uz,=!,ֈ׈YRw; ө<]դ毹yv97@Fas?H% %  y}(rR EA]}]P_"D H`uA~A-uA+Swi n,`N@1n\`P,dc3@~9Jpˤښu,a]> ?"6HF W$"K J)_I rFE@|_TQOYPO jC=} eR^~E؝ai$H[ R.(/X5b5_#m5xMx2 @ + WG@VB7ଽ8 yM)'^n& , @"}?hY_g>)1UeA-HAl3D5@  ah! y'7u~'ږ͕>&uU(撓A@ȂJ~ލg(-nJۥ1."]h+-uʺ*5|qO=/t> x, HHJK(Q$NT]-KA ,;,EirVbZ+QN,g @Ȃ҈ˡsI7j--fJ+ <](ԕf*k|HqgM{C]yrnp]c˸zӯ^X? lCy[fA)Y* 7 K,;,hdK̬h@7ʲ\v9C@&@6ZCϚɶ7å'gs+ =߉kiC: .I*/Qŝ"[n#(hr͖Pn\,k$OeYt_dk, lDz `X +duAqU +Y]п:XOTς,J֗sC ܘa@t Koų`m=w}/yٯ%<+-VITUeU g {FQL>^#9{&K/1RuA <{G>J'⠾=g4Vt/@G1%  J/Gjއzz[JM}<'+nfHJL˥('tٲPE[Uv勂R(:*7Kn/ +ZG])9{lEY],U_dY}Rkz [ V0t4Ȯ^4g|K9{$s Q,(:xNT"XA+,FNrgaUhY;JLeMUJUyQr^9r}E+%goꂬ}Z_C,'oE;zX&r>ʱyt@HYPq9t +>.EUf*V5Ww +Hn!AϮܼqI}QeA|UU,I$>mJc׭}aB?3(3F@ dA(pƂ mKoYdq}ʶ!{iob_fK boIǥ 2:_TFTi*h}QPj|fٕ7UViTvuYh%=/T-XiYek-I~vA7y+|s G,(=5kx!-#m=TQR+Ɩ }WUu +OsE=JREU-)7VnG޶ gWJn8BCgA+_xI,hR1lVyo_vl1cA.&7oܝo@ goC1hkEAz|,+V_a–X4tt* +,Yob쓻XQ.*S]+ +(7w + gWJn8t>1T]]QVE>b5bYQZ,j,#zFbF7-Rn9g @Ȃ%qN7>)+ǒ=Z< -*luߏw/#Ku^2[FitH:Յ])VK>ȥ~e7Niuβh2 -Y֧,hBKn/H{^z7,ȮXZJmܼr{9{@(@F\-,r:/+xHn$H#m#>H0j9fc/걢҉bJ[Nmf{RNA.{*Jϲ [#f5fK-sK[huA]zF+ck3] GoLƇ @ȂB|Q0o"#"iZw`L'X&rUA=U9@ΊF&ׅXr#QlY%_l +U  J.3pl'wkݜڧ@^XOFQ|S]~0[Swka9X+xK 1EAet)**Ve)(2cH9}tk̂* UjbLg,.f_dmbft2}r  +1;=u?je']m+uP"UdWJNAVc>/.@A`ɚu@}XK_d5B>~):l4:|}S#/TTmTQ=ASɍt1[#f5E[]ӱ,TCs㫆Gk@@ mBmά*TVdOQmj]TVbamKM)Ȋ|HwTQPG)Uj=JzSŎY]tJlTvGr0ׅ֫V[u3~@nA,xj ;fUާ^*﬊֪lϱ*ӦGۇَ`Mm?PGw kGI_E|[!q7uZ7DVq>L:gAPJ%)/];ÿ!pP@@ȂnR{g>l\*YrMu53qio,Yd;+}Qlw_E$+ +l0 zHH߿{4E/ +cnT-9޹ pC݊E@: +.{g|Nw$W/xrW/۞ۚ x? 5^Vo#ISXg_dEA o"dۇ-MJkm(PT' Hn~]p܈g ~5OD@ ->~8+x)8Y %IULl\ljmmwl={gdU?)/ +HGET_U8dEAVt) ?"&n0cE@ dA!@]W&='{lh&P1l 6,_cm( 0n$H"Y4d(Id+ +zGInKɊl:\8N.2ؐS k! dA9~;#.rMS:l`(Kr;t@gmcn%\VNAV3ӗK??~$aͥ֒mC/=(=拂&JcHnR޹E0Mn?# @ ++' 3eޓ)7En3rY~*全}4ŘH&)۪ kclHXcTF[QPFTcqpjlB0I @ ʮl~S*몫t.1鄒Wْ갧/+ +#u(X"d[YQi/ +"u~}*!pc( S,(;xם)_4UnHU>U_jg]i͒N%;;'YaHg}QА}SEA|]P_WOOsܞ?S׊= 4YP0""LrV4KnDEAVtUݺ^[\3*}H:l_e[۟~_0G*ZJ:eʉy>a=ƞS8+ @ ʍy,Δ+yrȽ!gȏKY!ҞYb3)ԑɗ^Z[F7zC,j/km](Ȋl0[f8Ȋ2?n!gYS5@ ʱ ]w>REA;Ԃ:_3~={uY~yYҦ1EA[:eEX3wnYhB;u @0 XtJF{Z.)FT}5,󉵱gBn,TN @'rDdA!9"'hN!RB@ +ݔ' #'ܙkδB@_)@+x |b)@@0 .@ 2@@@ GȂrd9M@~';\>rȍ{Jy  udAב [A3WKTBw*+wCrɍ$ʝk3E@ @ r8ZMtHeqPi UeGz{PnܓdAp9p  P\ _sn|Ӑ/k!dAPE:F#1`6щ:LgKtQE{UvVTUqpL@@ Kj;5tkSk0EyE@F j)HH?D)q*("-mKmUNeUazrς7  d@uSGjT|M(( иA럾WZZ"-jULkڔ1&ThNֺZݭ{2## %Jx?k[{~mG2a |ʼ AYy|(}"-똖iuǴ!-Y=LЉ:RT]*,ם6AYyEpR  |5Q50_Hx@L ʄY1.)#/}gQ-ۨ%YuPiWS-֡23Lu8  c5C_diɂB] ѧ0^ MHh2 TZ(}7Zך$EebuΗJsV  @]Ix=^,y9@K>4J'$MސfIIs Oieq-kE:T'&JW"JpL@~TsMsͷ +=^†/+"4APKO8hdB/K}4Dz i/ Zwebqm/B.q\.1C@ETs}R[S_/@[ g~^t4@zAä@) (M+fJ}u'dQAzG\:TY޲GA@rC xv">0 P/Aظ uJ8t4TzLzگWj{Q͋jaT b>PlcL;b&:^LԸɫ# @Ry/'>= ' c  #@t;zͤ;.R7W$ q=+=WM+lO1[&QjLZզiW>)P&*c@@<\H0YPAK $8UUc~;n$Hl)fADںYikςVE.1ƚI^@'+k6aFdAPLB݇`¬ +dB~$Hw8VJRMߒ>PrsE7ҊG >$ @us/jj\_e00@Au˄,6şE?ɏ5 oI㠿 F=Ŭɖ*YduAJ Z-LXƀ >ީ5 t33[!l'E rOʍ{Yur->*a[GhkdQmٷG`TYi5bYi˂Iػeg@9  6r`fS1R]-QyKTGa {gA^%ܗr֧=" +e̶σ_EKuDJ, ?5'؏?^J3>: @@ dAi MK]Qӱ|.PiwRe~rCF=#7Vn >Fnf=|<Ϧ~eol=lS#-#CVcY 8zG2Vd?n-, ѶLֈ8 +  @ eݔrB P/o.bږq)bmmtvSeOr=&xWޖPnbg'쇛$$M˻,zJ +' odȲ +J1kdYe̞`KI~Y0~@@ dAi J[eߘݨ>jA]L?ĵP{ uH'LKk̂ʻY4@2-g ĬY;YB@P^ ]cV3]y(_?#\z@Ȳ d@ A$#FjdAo@@ ;Ȃc9 /T^+՘f xռ<}ӷqk}\;gYPN4י:NW;Y [ 6Nn_ 'r @lYP}R6}lUL#V J8ȖY ˂lC?,Ȃ t޲ {ꂲs@@HYPy@ͦ+/زEf4/_ 8_k׮B+ҡm3wuWiU Z 6U-r_5|Ne0CUHJJs ˂"AA$xo +fَbDȖYclC?,gA,Ȟ0U@@@,( &S@: րŊ."ST/ZdiL_Ǵ"1mkg5:Z, n=$gYsr/M >EA+^)ʶFJH_I y\eAVfA% ˂:?ڪRY_F qA&mf  f4sxDR[ʂl5MeAOG|Tz5Ooa7 ӪMXі:ytԅr !ʧUr3}Qu^GL~L+l?"biςޒqPHP t4(m$n}|Q?= ~"yE@@ dACnAJ):Z +J,RkĬOȨF5%fAQ-jm"Zʹݩuž**/|Qr_dSD>?ziW-AAo>-#A4(5D &0 z-%!˅SC@,@f`!m-VX5ƚk{dUQPYPTG5/YӪP-%\ZX訳]uWU>"Xɾk{re7 vsuAMlXH`AM#A<(<->w +KAvZ1[KFT Y,(̳@}׺kb;.=,IEC +كC|{^[ f͂&E}FiYiCS(־bmu.][U_v{M&>/u0O{T6Kk4Y + >5ȚH4o(dj2.N[˵E@p  @ȂN @# h]}]+,_c}T"dmxgT{!=MV"m*֏Ssj{w)g;Y@yIroPHSy/{X':ZњMikTb>gADVYd٥;_C [̘yLj@@B)@ic P,iokNLVdQWB0ۆJ,o,JAU_z%;b_MVjCm;EA[R[, rȍۇ>6%wN:WMu6x4hG6Ǵ:PIK|נ/\o"t_,.]nZ7@@Ȃ@V\Ou났AD>\HǑ#Eߊ F-d_1˚Ӹky\ +źFosSYG=(g]M3{LuŶJLsBjbA?HWu jC1[&fYEY,iYZ*C@YP'!#ϗ^YP² [#4m-걕bV"_D?YDߍ <͵󵤪kb-; δӥTfA/t>G;х6t+o#tjwvD]IeUbL˂ +K_6  @Ȃ @ Z[XyU]4y#_F+;dqdbgQ{15@jsv5M.;J'Թt*z JFz8\G즊N*KW:b{kӶm z7/G ׈Y] ΂lo&[ f(XueeYՇQ-jqLŵGBǬK[gv. < w*EY[&Z'DP\,YJ]D +t#NC@HYPZy98ԋ@ys]h-u2(&uTɽjdYS , 8Yh@UQ fEA,;bl0J>O_Y + +צviI˹r%A2Ywr% VJAUCbGovڞb:RESY~#P]  "MCE 7:]iӾ␭j]ڒyZ7U[r , ^XO , +$ ~o"feB5eVEAQ},:V}E:VMIrgaU A=u.w%k0F[:j ckW@@t +Sc# tWAgYkk1m,(*ۉkid ZGH0o"fS͂jvEBV,dy8fyvvOl-:2    J+GE[&7EnBuAt][g;ԝ:RJ;,~U Ħ?V=<  օ#uK ìŐ\@@@ dA?F@fɽ%{7MW*/;E;Dj}%irZ+-=|9L;پ`OKAXC*ѐ&Ha   pdAnO @= |$7GM9 zQcfA#Tӕ>]g;d;i-Yrض|mVI+=ւA}dt@@YP# , zWndqr=*t߭3wd[mC͒]iNZ!ٞ_%=5QXt2օ-uEA<@@YPA@Bs͖{/Ϫ U`]=tjؑ:T+Q7yѕnQLs-f^&ZXSМfH@@@ mdAi P%Wn/ +Z!Al5T>@t.uЅ:uGr+E\ ,__i~4|ա?   o,T{d @z~,}VFUQs(aU RٽMW:RKkcE:P]hۇE(@@@ CnRsKOP.UrTu +zw +z^IrVY{]m+E:|R@@@ ddA!0@SAK徐'gEA3Vu +zTn**:.4' 43f@@YP8Q"aM[[QCG)   @8Ȃ1O@@@P    @8Ȃ1O@@@P    ,{hGm0     PpdA0 @@@I'H&#YP&c@@@@>>jē% jD|^@@@I(uv=(N((   d@mOm75<   Y#P[SxdAK#   @x2YP    jo% j)`    Rτ$ ʄY`     :ϐ$ ʐ`    .33" IaH    >oCb> @@@rM:",dAa)Ɖ   #P.Z ݀3g     :MF@@@,@Tg:~@@@YP覌#   u 3?   N,(tSƀ@@@@: ՙD@@@B'@)c    @ȂL"    +ݔ1`@@@@dAu@@@@ n0    Pg:    @ȂB7e @@@YPA@@@@ tdA2   Y,t     :MF@@@,@Tg:~@@@YP覌#   u 3?   N,(tSƀ@@@@: ՙD@@@B'@)c    @ȂL"    +ݔ1`@@@@dAu@@@@ n0    Pg:    @ȂB7e @@j{?_ @j +/5  4@m578 @Ȃ8k@@ $9}Sc ! +d1T@@ -ҹFdx@ MI,E@5INm9 9 - 疸x2  @jty!ϑ7 k!z @@.pMS۷y!yaT @ ''SC@+P[s=yCd @)c  @xOrnߎ: ɆI2?f]WECU 08DxNycqyrR  @Xg( @gA|n,V 2J*I @.|)]~ݪA+ +]zjv +x  @rew{%5xO9'P.bt9Q'.J& Aީ'@TkWQP*v{˙Ա/_ @Ob1>mM4S&ީ'@;TKqLFn|0H}],q2[:QS @H%G{'O(X ?%TvSy5Nkqښ63n'G= 8Xۓ*O*I @yɷPGI] @@<˳73 -8zsj=g2 @97g @@ b}O X(.b pZ4  @X 2§ezB]mWГ @yҎW)frq/M  @99w @*^uA; +bO@Dz}(z5kL9#\V  @S?;zyyZJ ϖUkUڍ^> +V6qdMە&5j"@X-\nF<ql +Vz8JN_HO/_ +JxZUWInwE<\v/L{r缠! @cuE(# G\K1W +Tsu1c7^> + @ oxSt7Q&fγl|@|s&ms̴>W! @7 Xn{wafNg;q&yl;S˸˚uVc7 v%#%@o@5yܧ7Yk b;VS/QE{f8kJ`K + @Uvo0/񟺄qC^/sx5Ҝio+e @xZ^oF0m9<*&@Hͼfepf-4۶imݙVc/˪Cv  @zi;$r <ȵx +9 @O7K鎜x;Om 2 @/[ȯۃMxwSç*K4߶ C @d,ߞ?rnUkx+h=̘m>ͷ- @J{df}袊  @|*>w{M?hra 155U O%bz,gj3Q, @ 0[ -Gg_bvyEݍ~gmw;jmL%0ݴZK @k\Ry7ԛ.tϛaoHWmL%0Gmu] @ ++8]+/⸩>/jWM%xPx>V OmzRإeUv  @T²WvCT˭Vw;n@[^!@ @`w;I׫I[Y׏7TaS٠݇zJ5 @+R?I=y^/ -O\ǴAR/n #@ @䆽zMV.>{n̂h 2 @?Ho(?Ë;7w/r>Zח{ @ \߿xpWO4{T y7m>.'@ @eSPSy5AЧeI} @Mz${((8-4 Л]?a,_Cf|.A @`XxC52I p @@+'d @ pNh]eg%py7=^ @(b%JY G98/Dt- @|*o"D m&Ń䳸?] m5݂~ |ۂ  @>tVJfrG~wq"z^d/bCV扒 @8!Pqqn͌':SދGgh/8or/mVQSb2 @.|)Rġ^> +Y A 8̗q9^Y0vA"C&@ @B"Ź,r$;lG\ +xx@9Eq9^Y0ee 2 @'Wd[ִWm^ +Tj7FG @~"zܥ;]/(SY".ԛz8Q@ @7|km& Y Dccj^/6*v{R @~+W%nN'?2hˣ9nkVTPlj @@ .#Iu@+P7m:mvLKT zt @ s\=pe%C5m& @ @k6?DTj7:G:A,:PFJ @J:Q}QNhAG @ @(+|q/V>Sҵq5:d @ E9Q @ @L|ȇtfKZv @ E`ljKn&RPK @V""Vơ0:v @ @DD2Ԯ޹#OeYF @ 0CS+܍^~F'I @ p\ m>s$;xgT @ @O@姞*YΔezFL @ pO;dhA @ 0&̸YG  @ @&Jݴ[ @ @@'N?! @ @zXP +84}&@ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ pDd3L +endstream +endobj +8 0 obj +109042 +endobj +9 0 obj +<< /Length 10 0 R /Type /XObject /Subtype /Image /Width 1539 /Height 29 /ColorSpace +12 0 R /BitsPerComponent 8 /Filter /FlateDecode >> +stream +xЁ Pa 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0` 0`N? +endstream +endobj +10 0 obj +606 +endobj +11 0 obj +<< /Type /ExtGState /OPM 1 >> +endobj +13 0 obj +<< /Length 14 0 R /N 3 /Alternate /DeviceRGB /Filter /FlateDecode >> +stream +xwTSϽ7" %z ;HQIP&vDF)VdTG"cE b PQDE݌k 5ޚYg}׺PtX4X\XffGD=HƳ.d,P&s"7C$ +E6<~&S2)212 "įl+ɘ&Y4Pޚ%ᣌ\%g|eTI(L0_&l2E9r9hxgIbטifSb1+MxL 0oE%YmhYh~S=zU&ϞAYl/$ZUm@O ޜl^ ' lsk.+7oʿ9V;?#I3eE妧KD d9i,UQ h +A1vjpԁzN6p\W p G@ +K0ށiABZyCAP8C@&*CP=#t] 4}a ٰ;GDxJ>,_“@FXDBX$!k"EHqaYbVabJ0՘cVL6f3bձX'?v 6-V``[a;p~\2n5׌ &x*sb|! +ߏƿ' Zk! $l$T4QOt"y\b)AI&NI$R$)TIj"]&=&!:dGrY@^O$ _%?P(&OJEBN9J@y@yCR nXZOD}J}/G3ɭk{%Oחw_.'_!JQ@SVF=IEbbbb5Q%O@%!BӥyҸM:e0G7ӓ e%e[(R0`3R46i^)*n*|"fLUo՝mO0j&jajj.ϧwϝ_4갺zj=U45nɚ4ǴhZ ZZ^0Tf%9->ݫ=cXgN].[7A\SwBOK/X/_Q>QG[ `Aaac#*Z;8cq>[&IIMST`ϴ kh&45ǢYYF֠9<|y+ =X_,,S-,Y)YXmĚk]c}džjcΦ浭-v};]N"&1=xtv(}'{'IߝY) Σ -rqr.d._xpUەZM׍vm=+KGǔ ^WWbj>:>>>v}/avO8 +FV> 2 u/_$\BCv< 5 ]s.,4&yUx~xw-bEDCĻHGKwFGEGME{EEKX,YFZ ={$vrK +.3\rϮ_Yq*©L_wד+]eD]cIIIOAu_䩔)3ѩiB%a+]3='/40CiU@ёL(sYfLH$%Y jgGeQn~5f5wugv5k֮\۹Nw]m mHFˍenQQ`hBBQ-[lllfjۗ"^bO%ܒY}WwvwXbY^Ю]WVa[q`id2JjGէ{׿m>PkAma꺿g_DHGGu;776ƱqoC{P38!9 ҝˁ^r۽Ug9];}}_~imp㭎}]/}.{^=}^?z8hc' +O*?f`ϳgC/Oϩ+FFGGόzˌㅿ)ѫ~wgbk?Jި9mdwi獵ޫ?cǑOO?w| x&mf +endstream +endobj +14 0 obj +2612 +endobj +12 0 obj +[ /ICCBased 13 0 R ] +endobj +3 0 obj +<< /Type /Pages /MediaBox [0 0 612 792] /Count 1 /Kids [ 2 0 R ] >> +endobj +15 0 obj +<< /Type /Catalog /Pages 3 0 R >> +endobj +16 0 obj +(Mac OS X 10.7.4 Quartz PDFContext) +endobj +17 0 obj +(D:20120614050627Z00'00') +endobj +1 0 obj +<< /Producer 16 0 R /CreationDate 17 0 R /ModDate 17 0 R >> +endobj +xref +0 18 +0000000000 65535 f +0000113547 00000 n +0000000242 00000 n +0000113320 00000 n +0000000022 00000 n +0000000223 00000 n +0000000346 00000 n +0000000475 00000 n +0000109685 00000 n +0000109707 00000 n +0000110481 00000 n +0000110501 00000 n +0000113283 00000 n +0000110547 00000 n +0000113262 00000 n +0000113403 00000 n +0000113453 00000 n +0000113505 00000 n +trailer +<< /Size 18 /Root 15 0 R /Info 1 0 R /ID [ <4e77d5a96b07c7a3f06a11c13456bf6a> +<4e77d5a96b07c7a3f06a11c13456bf6a> ] >> +startxref +113622 +%%EOF diff --git a/doc/images/example1.pdf b/doc/images/example1.pdf new file mode 100644 index 000000000..e9446f0fe Binary files /dev/null and b/doc/images/example1.pdf differ diff --git a/doc/images/example2.pdf b/doc/images/example2.pdf new file mode 100644 index 000000000..8fa600eda Binary files /dev/null and b/doc/images/example2.pdf differ diff --git a/doc/images/hmm-FG.pdf b/doc/images/hmm-FG.pdf new file mode 100644 index 000000000..e522f8cc3 Binary files /dev/null and b/doc/images/hmm-FG.pdf differ diff --git a/doc/images/hmm.pdf b/doc/images/hmm.pdf new file mode 100644 index 000000000..38c8f0623 Binary files /dev/null and b/doc/images/hmm.pdf differ diff --git a/doc/images/littleRobot.pdf b/doc/images/littleRobot.pdf new file mode 100644 index 000000000..e19fc0edf Binary files /dev/null and b/doc/images/littleRobot.pdf differ diff --git a/doc/images/sphere2500-result.pdf b/doc/images/sphere2500-result.pdf new file mode 100644 index 000000000..9175f4ea1 Binary files /dev/null and b/doc/images/sphere2500-result.pdf differ diff --git a/doc/images/w100-result.pdf b/doc/images/w100-result.pdf new file mode 100644 index 000000000..d09607a06 Binary files /dev/null and b/doc/images/w100-result.pdf differ diff --git a/doc/math.lyx b/doc/math.lyx index 88e243a01..b579d3ea4 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5189,7 +5189,7 @@ R_{2}^{T}\left[t_{2}-t_{1}\right]_{\times}R_{1} & -R_{2}^{T}R_{1} and in its second argument, \begin_inset Formula \begin{eqnarray*} -\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6} +\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{2}} & = & I_{6} \end{eqnarray*} \end_inset diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile new file mode 100644 index 000000000..33aa1ab96 --- /dev/null +++ b/docker/ubuntu-boost-tbb-eigen3/Dockerfile @@ -0,0 +1,18 @@ +# 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/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 676dd42ec..204af1fea 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -71,18 +71,14 @@ int main(int argc, char* argv[]) { // add measurement factors SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5)); boost::shared_ptr factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0)); /* 3. Create an initial estimate for the camera pose */ Values initial; diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp new file mode 100644 index 000000000..8331ade04 --- /dev/null +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -0,0 +1,120 @@ +/** + * @file ISAM2Example_SmartFactor.cpp + * @brief test of iSAM with smart factors, led to bitbucket issue #367 + * @author Alexander (pumaking on BitBucket) + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::P; +using symbol_shorthand::X; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +int main(int argc, char* argv[]) { + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + Vector6 sigmas; + sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1); + auto noise = noiseModel::Diagonal::Sigmas(sigmas); + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + parameters.cacheLinearizedFactors = false; + parameters.enableDetailedResults = true; + parameters.print(); + ISAM2 isam(parameters); + + // Create a factor graph + NonlinearFactorGraph graph; + Values initialEstimate; + + Point3 point(0.0, 0.0, 1.0); + + // Intentionally initialize the variables off from the ground truth + Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20)); + + Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0)); + Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0)); + Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0)); + Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0)); + Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0)); + + vector poses = {pose1, pose2, pose3, pose4, pose5}; + + // Add first pose + graph.emplace_shared>(X(0), poses[0], noise); + initialEstimate.insert(X(0), poses[0].compose(delta)); + + // Create smart factor with measurement from first pose only + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); + smartFactor->add(PinholePose(poses[0], K).project(point), X(0)); + graph.push_back(smartFactor); + + // loop over remaining poses + for (size_t i = 1; i < 5; i++) { + cout << "****************************************************" << endl; + cout << "i = " << i << endl; + + // Add prior on new pose + graph.emplace_shared>(X(i), poses[i], noise); + initialEstimate.insert(X(i), poses[i].compose(delta)); + + // "Simulate" measurement from this pose + PinholePose camera(poses[i], K); + Point2 measurement = camera.project(point); + cout << "Measurement " << i << "" << measurement << endl; + + // Add measurement to smart factor + smartFactor->add(measurement, X(i)); + + // Update iSAM2 + ISAM2Result result = isam.update(graph, initialEstimate); + result.print(); + + cout << "Detailed results:" << endl; + for (auto keyedStatus : result.detail->variableStatus) { + const auto& status = keyedStatus.second; + PrintKey(keyedStatus.first); + cout << " {" << endl; + cout << "reeliminated: " << status.isReeliminated << endl; + cout << "relinearized above thresh: " << status.isAboveRelinThreshold + << endl; + cout << "relinearized involved: " << status.isRelinearizeInvolved << endl; + cout << "relinearized: " << status.isRelinearized << endl; + cout << "observed: " << status.isObserved << endl; + cout << "new: " << status.isNew << endl; + cout << "in the root clique: " << status.inRootClique << endl; + cout << "}" << endl; + } + + Values currentEstimate = isam.calculateEstimate(); + currentEstimate.print("Current estimate: "); + + boost::optional pointEstimate = smartFactor->point(currentEstimate); + if (pointEstimate) { + cout << *pointEstimate << endl; + } else { + cout << "Point degenerate." << endl; + } + + // Reset graph and initial estimate for next iteration + graph.resize(0); + initialEstimate.clear(); + } + + return 0; +} diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp new file mode 100644 index 000000000..b4ad5d574 --- /dev/null +++ b/examples/ImuFactorExample2.cpp @@ -0,0 +1,134 @@ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Shorthand for velocity and pose variables +using symbol_shorthand::V; +using symbol_shorthand::X; + +const double kGravity = 9.81; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + auto params = PreintegrationParams::MakeSharedU(kGravity); + params->setAccelerometerCovariance(I_3x3 * 0.1); + params->setGyroscopeCovariance(I_3x3 * 0.1); + params->setIntegrationCovariance(I_3x3 * 0.1); + params->setUse2ndOrderCoriolis(false); + params->setOmegaCoriolis(Vector3(0, 0, 0)); + + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + + // Start with a camera on x-axis looking at origin + 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 pose_0 = camera.pose(); + + // Now, create a constant-twist scenario that makes the camera orbit the + // origin + double angular_velocity = M_PI, // rad/sec + delta_t = 1.0 / 18; // makes for 10 degrees per step + Vector3 angular_velocity_vector(0, -angular_velocity, 0); + Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0); + auto scenario = ConstantTwistScenario(angular_velocity_vector, + linear_velocity_vector, pose_0); + + // Create a factor graph + NonlinearFactorGraph newgraph; + + // Create (incremental) ISAM2 solver + ISAM2 isam; + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate, totalEstimate, result; + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 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)); + + // 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); + 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); + + initialEstimate.insert(V(0), n_velocity); + + // IMU preintegrator + PreintegratedImuMeasurements accum(params); + + // Simulate poses and imu measurements, adding them to the factor graph + for (size_t i = 0; i < 36; ++i) { + double t = i * delta_t; + if (i == 0) { // First time add two poses + auto pose_1 = scenario.pose(delta_t); + initialEstimate.insert(X(0), pose_0.compose(delta)); + initialEstimate.insert(X(1), pose_1.compose(delta)); + } else if (i >= 2) { // Add more poses as necessary + auto pose_i = scenario.pose(t); + initialEstimate.insert(X(i), pose_i.compose(delta)); + } + + if (i > 0) { + // Add Bias variables periodically + if (i % 5 == 0) { + biasKey++; + Symbol b1 = biasKey - 1; + Symbol b2 = biasKey; + Vector6 covvec; + covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; + auto cov = noiseModel::Diagonal::Variances(covvec); + auto f = boost::make_shared >( + b1, b2, imuBias::ConstantBias(), cov); + newgraph.add(f); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + } + // Predict acceleration and gyro measurements in (actual) body frame + auto measuredAcc = scenario.acceleration_b(t) - + scenario.rotation(t).transpose() * params->n_gravity; + auto measuredOmega = scenario.omega_b(t); + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t); + + // Add Imu Factor + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); + newgraph.add(imufac); + + // insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity); + accum.resetIntegration(); + } + + // Incremental solution + isam.update(newgraph, initialEstimate); + result = isam.calculateEstimate(); + newgraph = NonlinearFactorGraph(); + initialEstimate.clear(); + } + GTSAM_PRINT(result); + return 0; +} +/* ************************************************************************* */ diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 84bca65f3..17f921fd1 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -120,15 +120,15 @@ int main(int argc, char** argv) { // 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)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + 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 - graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + 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 // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index eb6bdd49d..3416eb6b7 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -37,12 +37,12 @@ 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)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(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)); - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print Values initial; diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index b5cd681e5..3b547e70c 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -65,15 +65,15 @@ int main(int argc, char** argv) { // 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.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(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)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index a716f9cd8..7c43c22e2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -81,13 +81,13 @@ int main(int argc, char** argv) { // 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.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.emplace_shared >(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 - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + 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 @@ -101,9 +101,9 @@ int main(int argc, char** argv) { range32 = 2.0; // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 7991f7fbf..f977a08af 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -72,23 +72,23 @@ 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.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(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)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), 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); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, // 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.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, 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 diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 314ccbdb4..99711da2d 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -33,19 +33,19 @@ int main (int argc, char** argv) { // 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.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(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)); // 2b. Add odometry factors - graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), 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); // 2c. Add the loop closure constraint - graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); // 3. Create the data structure to hold the initial estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2c25d2f8e..9b459d7b8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -36,18 +36,18 @@ int main(int argc, char** argv) { // 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.push_back(PriorFactor(1, prior, priorNoise)); + graph.emplace_shared >(1, prior, priorNoise); // 2b. Add odometry factors noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + 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.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 893454936..0e48bb892 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -75,14 +75,14 @@ int main(int argc, char* argv[]) { // 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.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // add directly to 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); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } } @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { // 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.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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 diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 4112afcad..e9d02b15c 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -82,13 +82,13 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); + graph.emplace_shared >(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.push_back(PriorFactor(1, poses[1], noise)); // add directly to graph + graph.emplace_shared >(1, poses[1], noise); // add directly to graph graph.print("Factor Graph:\n"); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 7f6c0ba71..da7c5ba9b 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -74,10 +74,10 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); + graph.emplace_shared >(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, poses[0], noise)); + graph.emplace_shared >(1, poses[0], noise); // Create the initial estimate to the solution Values initialEstimate; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 4c655fb7a..a3a416eb3 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -59,15 +59,15 @@ int main (int argc, char* argv[]) { for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(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.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + 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)); // Create initial estimate Values initial; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 44a402b33..13a07dda5 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -64,15 +64,15 @@ int main (int argc, char* argv[]) { for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(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.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + 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)); // Create initial estimate Values initial; diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 1c5d155dc..93e010543 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // 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); @@ -65,17 +65,17 @@ int main(int argc, char* argv[]) { 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.push_back(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + 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.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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.push_back(PriorFactor(Symbol('K', 0), K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index deaf3b781..63c512edb 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -206,7 +206,7 @@ int main(int argc, char *argv[]) { } #ifdef GTSAM_USE_TBB - std::auto_ptr init; + std::unique_ptr init; if(nThreads > 0) { cout << "Using " << nThreads << " threads" << endl; init.reset(new tbb::task_scheduler_init(nThreads)); diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 35d6747bf..27c10deb3 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv){ //create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; Pose3 first_pose; - graph.push_back(NonlinearEquality(1, Pose3())); + 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); @@ -47,14 +47,14 @@ int main(int argc, char** argv){ 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.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); + graph.emplace_shared >(StereoPoint2(120, 80, 440), model, 1, 4, K); + graph.emplace_shared >(StereoPoint2(320, 280, 140), model, 1, 5, K); //create and add stereo factors between second pose and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + graph.emplace_shared >(StereoPoint2(570, 520, 490), model, 2, 3, K); + 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 Values initial_estimate; diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 8b88c772d..80831bd21 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -83,9 +83,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //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.push_back( - GenericStereoFactor(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K)); + 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 if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); @@ -99,7 +99,7 @@ int main(int argc, char** argv){ //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.push_back(NonlinearEquality(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 diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index aa2195078..178fa513f 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -20,6 +20,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 78bc463ec..751b776f6 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -11,8 +11,8 @@ /** * @file VisualISAM2Example.cpp - * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset - * This version uses iSAM2 to solve the problem incrementally + * @brief A visualSLAM example for the structure-from-motion problem on a + * simulated dataset This version uses iSAM2 to solve the problem incrementally * @author Duy-Nguyen Ta */ @@ -25,27 +25,28 @@ // For loading the data #include "SFMdata.h" -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// 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 +// 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 -// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so -// include iSAM2 here +// We want to use iSAM2 to solve the structure-from-motion problem +// incrementally, so include iSAM2 here #include -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors +// iSAM2 requires as input a set of new factors to be added stored in a factor +// graph, and initial guesses for any new variables used in the added factors #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. +// 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 @@ -56,12 +57,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 measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // Create the set of ground-truth landmarks vector points = createPoints(); @@ -69,10 +69,12 @@ int main(int argc, char* argv[]) { // Create the set of ground-truth poses vector poses = createPoses(); - // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization - // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter - // structure is available that allows the user to set various properties, such as the relinearization threshold - // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps + // to maintain proper linearization and efficient variable ordering, iSAM2 + // performs partial relinearization/reordering at each step. A parameter + // structure is available that allows the user to set various properties, such + // as the relinearization threshold and type of linear solver. For this + // example, we we set the relinearization threshold small so the iSAM2 result // will approach the batch result. ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; @@ -83,44 +85,52 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; Values initialEstimate; - // Loop over the different poses, adding the observations to iSAM incrementally + // Loop over the 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) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } // Add an initial guess for the current pose // Intentionally initialize the variables off from the ground truth - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); - // 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 - // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before - // adding it to iSAM. - if( i == 0) { - // Add a prior on pose x0 - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + // 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 Also, + // as iSAM solves incrementally, we must wait until each is observed at + // least twice before adding it to iSAM. + if (i == 0) { + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) + .finished()); + graph.emplace_shared >(Symbol('x', 0), poses[0], + kPosePrior); // Add a prior on landmark l0 - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared >(Symbol('l', 0), points[0], + kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); 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] + kDeltaPoint); } else { // Update iSAM with the new factors isam.update(graph, initialEstimate); - // Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. - // If accuracy is desired at the expense of time, update(*) can be called additional times - // to perform multiple optimizer iterations every step. + // Each call to iSAM2 update(*) performs one iteration of the iterative + // nonlinear solver. If accuracy is desired at the expense of time, + // update(*) can be called additional times to perform multiple optimizer + // iterations every step. isam.update(); Values currentEstimate = isam.calculateEstimate(); cout << "****************************************************" << endl; diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index a839289c2..8ca1be596 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -89,9 +89,8 @@ int main(int argc, char* argv[]) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement - graph.add( - GenericProjectionFactor(measurement, noise, - Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, noise, + Symbol('x', i), Symbol('l', j), K); } // Intentionally initialize the variables off from the ground truth @@ -109,12 +108,12 @@ int main(int argc, char* argv[]) { // 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( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/gtsam.h b/gtsam.h index 7c5bc99b1..24a717c3c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2,9 +2,13 @@ * GTSAM Wrap Module Definition * - * These are the current classes available through the matlab toolbox interface, + * These are the current classes available through the matlab and python wrappers, * add more functions/classes as they are available. * + * IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the + * argument names matter. An implementation restriction is that in overloaded methods + * or functions, arguments of different types *have* to have different names. + * * Requirements: * Classes must start with an uppercase letter * - Can wrap a typedef @@ -16,7 +20,7 @@ * - Any class with which be copied with boost::make_shared() * - boost::shared_ptr of any object type * Constructors - * - Overloads are supported + * - Overloads are supported, but arguments of different types *have* to have different names * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB * Methods * - Constness has no effect @@ -26,7 +30,7 @@ * Static methods * - Must start with a letter (upper or lowercase) and use the "static" keyword * - The first letter will be made uppercase in the generated MATLAB interface - * - Overloads are supported + * - Overloads are supported, but arguments of different types *have* to have different names * Arguments to functions any of * - Eigen types: Matrix, Vector * - Eigen types and classes as an optionally const reference @@ -89,6 +93,18 @@ * - Add "void serializable()" to a class if you only want the class to be serialized as a * part of a container (such as noisemodel). This version does not require a publicly * accessible default constructor. + * Forward declarations and class definitions for Cython: + * - Need to specify the base class (both this forward class and base class are declared in an external cython header) + * This is so Cython can generate proper inheritance. + * Example when wrapping a gtsam-based project: + * // forward declarations + * virtual class gtsam::NonlinearFactor + * virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor + * // class definition + * #include + * virtual class MyFactor : gtsam::NoiseModelFactor {...}; + * - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class + * - This will cause an ambiguity problem in Cython pxd header file */ /** @@ -102,51 +118,106 @@ * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load */ -namespace std { - #include - template - class vector - { - //Do we need these? - //Capacity - /*size_t size() const; - size_t max_size() const; - //void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - T* at(size_t n); - T* front(); - T* back(); - - //Modifiers - void assign(size_t n, const T& u); - void push_back(const T& x); - void pop_back();*/ - }; - //typedef std::vector - - #include - template - class list - { - - - }; - -} - namespace gtsam { +// Actually a FastList +#include +class KeyList { + KeyList(); + KeyList(const gtsam::KeyList& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void pop_back(); + void pop_front(); + void sort(); + void remove(size_t key); + + void serialize() const; +}; + +// Actually a FastSet +class KeySet { + KeySet(); + KeySet(const gtsam::KeySet& set); + KeySet(const gtsam::KeyVector& vector); + KeySet(const gtsam::KeyList& list); + + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t key); + void merge(const gtsam::KeySet& other); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists + + void serialize() const; +}; + +// Actually a vector +class KeyVector { + KeyVector(); + KeyVector(const gtsam::KeyVector& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; + + void serialize() const; +}; + +// Actually a FastMap +class KeyGroupMap { + KeyGroupMap(); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); +}; + //************************************************************************* // base //************************************************************************* /** gtsam namespace functions */ +#include bool linear_independent(Matrix A, Matrix B, double tol); +#include virtual class Value { // No constructors because this is an abstract class @@ -254,6 +325,7 @@ class LieMatrix { // geometry //************************************************************************* +#include class Point2 { // Standard Constructors Point2(); @@ -262,7 +334,7 @@ class Point2 { // Testable void print(string s) const; - bool equals(const gtsam::Point2& pose, double tol) const; + bool equals(const gtsam::Point2& point, double tol) const; // Group static gtsam::Point2 identity(); @@ -279,6 +351,7 @@ class Point2 { }; // std::vector +#include class Point2Vector { // Constructors @@ -304,6 +377,7 @@ class Point2Vector void pop_back(); }; +#include class StereoPoint2 { // Standard Constructors StereoPoint2(); @@ -337,6 +411,7 @@ class StereoPoint2 { void serialize() const; }; +#include class Point3 { // Standard Constructors Point3(); @@ -360,6 +435,7 @@ class Point3 { void serialize() const; }; +#include class Rot2 { // Standard Constructors and Named Constructors Rot2(); @@ -403,10 +479,16 @@ class Rot2 { void serialize() const; }; +#include class Rot3 { // Standard Constructors and Named Constructors Rot3(); Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33); + static gtsam::Rot3 Rx(double t); static gtsam::Rot3 Ry(double t); static gtsam::Rot3 Rz(double t); @@ -418,6 +500,7 @@ class Rot3 { 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 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); // Testable void print(string s) const; @@ -457,10 +540,11 @@ class Rot3 { void serialize() const; }; +#include class Pose2 { // Standard Constructor Pose2(); - Pose2(const gtsam::Pose2& pose); + Pose2(const gtsam::Pose2& other); Pose2(double x, double y, double theta); Pose2(double theta, const gtsam::Point2& t); Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); @@ -484,7 +568,7 @@ class Pose2 { static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; + Vector Adjoint(Vector xi) const; static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 @@ -505,13 +589,14 @@ class Pose2 { void serialize() const; }; +#include class Pose3 { // Standard Constructors Pose3(); - Pose3(const gtsam::Pose3& pose); + Pose3(const gtsam::Pose3& other); Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); + Pose3(Matrix mat); // Testable void print(string s) const; @@ -520,23 +605,23 @@ class Pose3 { // Group static gtsam::Pose3 identity(); gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; // Manifold gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; // Lie Group static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); + static Vector Logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; + gtsam::Point3 transform_from(const gtsam::Point3& point) const; + gtsam::Point3 transform_to(const gtsam::Point3& point) const; // Standard Interface gtsam::Rot3 rotation() const; @@ -554,13 +639,14 @@ class Pose3 { }; // std::vector +#include class Pose3Vector { Pose3Vector(); size_t size() const; bool empty() const; gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& x); + void push_back(const gtsam::Pose3& pose); }; #include @@ -606,6 +692,7 @@ class EssentialMatrix { double error(Vector vA, Vector vB); }; +#include class Cal3_S2 { // Standard Constructors Cal3_S2(); @@ -658,11 +745,13 @@ virtual class Cal3DS2_Base { double py() const; double k1() const; double k2() const; + Matrix K() const; + Vector k() const; + Vector vector() const; // Action on Point2 gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; // enabling serialization functionality void serialize() const; @@ -754,7 +843,6 @@ class Cal3Bundler { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; // Standard Interface @@ -772,11 +860,12 @@ class Cal3Bundler { void serialize() const; }; +#include class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); + CalibratedCamera(Vector v); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable @@ -786,7 +875,7 @@ class CalibratedCamera { // Manifold static size_t Dim(); size_t dim() const; - gtsam::CalibratedCamera retract(const Vector& d) const; + gtsam::CalibratedCamera retract(Vector d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; // Action on Point3 @@ -801,6 +890,7 @@ class CalibratedCamera { void serialize() const; }; +#include template class PinholeCamera { // Standard Constructors and Named Constructors @@ -821,7 +911,7 @@ class PinholeCamera { CALIBRATION calibration() const; // Manifold - This retract(const Vector& d) const; + This retract(Vector d) const; Vector localCoordinates(const This& T2) const; size_t dim() const; static size_t Dim(); @@ -832,12 +922,13 @@ class PinholeCamera { gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); + double range(const gtsam::Pose3& pose); // enabling serialization functionality void serialize() const; }; +#include virtual class SimpleCamera { // Standard Constructors and Named Constructors SimpleCamera(); @@ -847,6 +938,8 @@ virtual class SimpleCamera { static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector); // Testable void print(string s) const; @@ -857,7 +950,7 @@ virtual class SimpleCamera { gtsam::Cal3_S2 calibration() const; // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; + gtsam::SimpleCamera retract(Vector d) const; Vector localCoordinates(const gtsam::SimpleCamera& T2) const; size_t dim() const; static size_t Dim(); @@ -868,7 +961,7 @@ virtual class SimpleCamera { gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); + double range(const gtsam::Pose3& pose); // enabling serialization functionality void serialize() const; @@ -878,10 +971,12 @@ virtual class SimpleCamera { // Some typedefs for common camera types // PinholeCameraCal3_S2 is the same as SimpleCamera above typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +// due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified +//typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +//typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +//typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +#include class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -897,7 +992,7 @@ class StereoCamera { gtsam::Cal3_S2Stereo calibration() const; // Manifold - gtsam::StereoCamera retract(const Vector& d) const; + gtsam::StereoCamera retract(Vector d) const; Vector localCoordinates(const gtsam::StereoCamera& T2) const; size_t dim() const; static size_t Dim(); @@ -959,7 +1054,6 @@ virtual class SymbolicFactorGraph { // Standard interface gtsam::KeySet keys() const; - void push_back(gtsam::SymbolicFactor* factor); void push_back(const gtsam::SymbolicFactorGraph& graph); void push_back(const gtsam::SymbolicBayesNet& bayesNet); void push_back(const gtsam::SymbolicBayesTree& bayesTree); @@ -982,13 +1076,13 @@ virtual class SymbolicFactorGraph { const gtsam::Ordering& ordering); pair eliminatePartialMultifrontal( const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); }; #include @@ -1034,14 +1128,14 @@ class SymbolicBayesTree { //Constructors SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); // Testable void print(string s); bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; + //size_t findParentClique(const gtsam::IndexVector& parents) const; size_t size(); void saveGraph(string s) const; void clear(); @@ -1086,9 +1180,9 @@ class VariableIndex { //template //VariableIndex(const T& factorGraph, size_t nVariables); //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); - VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); - VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& sfg); + VariableIndex(const gtsam::GaussianFactorGraph& gfg); + VariableIndex(const gtsam::NonlinearFactorGraph& fg); VariableIndex(const gtsam::VariableIndex& other); // Testable @@ -1133,12 +1227,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { }; virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); static gtsam::noiseModel::Constrained* All(size_t dim); static gtsam::noiseModel::Constrained* All(size_t dim, double mu); @@ -1321,12 +1415,12 @@ virtual class JacobianFactor : gtsam::GaussianFactor { pair jacobianUnweighted() const; Matrix augmentedJacobianUnweighted() const; - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; pair eliminate(const gtsam::Ordering& keys) const; - void setModel(bool anyConstrained, const Vector& sigmas); + void setModel(bool anyConstrained, Vector sigmas); gtsam::noiseModel::Diagonal* get_model() const; @@ -1377,11 +1471,12 @@ class GaussianFactorGraph { size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; bool exists(size_t idx) const; // Building the graph void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* factor); + void push_back(const gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianFactorGraph& graph); void push_back(const gtsam::GaussianBayesNet& bayesNet); void push_back(const gtsam::GaussianBayesTree& bayesTree); @@ -1420,13 +1515,13 @@ class GaussianFactorGraph { const gtsam::Ordering& ordering); pair eliminatePartialMultifrontal( const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); // Conversion to matrices Matrix sparseJacobian_() const; @@ -1496,7 +1591,7 @@ virtual class GaussianBayesNet { size_t size() const; // FactorGraph derived interface - size_t size() const; + // size_t size() const; gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; bool exists(size_t idx) const; @@ -1543,6 +1638,7 @@ virtual class GaussianBayesTree { gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; +#include class Errors { //Constructors Errors(); @@ -1553,6 +1649,7 @@ class Errors { bool equals(const gtsam::Errors& expected, double tol) const; }; +#include class GaussianISAM { //Constructor GaussianISAM(); @@ -1589,7 +1686,7 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete void setReset(int value); void setEpsilon_rel(double value); void setEpsilon_abs(double value); - void print(); + void print() const; }; #include @@ -1683,6 +1780,7 @@ class Ordering { void serialize() const; }; +#include class NonlinearFactorGraph { NonlinearFactorGraph(); NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); @@ -1693,6 +1791,8 @@ class NonlinearFactorGraph { size_t size() const; bool empty() const; void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor* factors); + void resize(size_t size); size_t nrFactors() const; gtsam::NonlinearFactor* at(size_t idx) const; void push_back(const gtsam::NonlinearFactorGraph& factors); @@ -1700,6 +1800,7 @@ class NonlinearFactorGraph { void add(gtsam::NonlinearFactor* factor); bool exists(size_t idx) const; gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; // NonlinearFactorGraph double error(const gtsam::Values& values) const; @@ -1713,6 +1814,7 @@ class NonlinearFactorGraph { void serialize() const; }; +#include virtual class NonlinearFactor { // Factor base class size_t size() const; @@ -1720,7 +1822,7 @@ virtual class NonlinearFactor { void print(string s) const; void printKeys(string s) const; // NonlinearFactor - void equals(const gtsam::NonlinearFactor& other, double tol) const; + bool equals(const gtsam::NonlinearFactor& other, double tol) const; double error(const gtsam::Values& c) const; size_t dim() const; bool active(const gtsam::Values& c) const; @@ -1729,8 +1831,9 @@ virtual class NonlinearFactor { // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen }; +#include virtual class NoiseModelFactor: gtsam::NonlinearFactor { - void equals(const gtsam::NoiseModelFactor& other, double tol) const; + bool equals(const gtsam::NoiseModelFactor& other, double tol) const; gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below gtsam::noiseModel::Base* noiseModel() const; Vector unwhitenedError(const gtsam::Values& x) const; @@ -1772,34 +1875,34 @@ class Values { // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; - void insert(size_t j, const gtsam::Point2& t); - void insert(size_t j, const gtsam::Point3& t); - void insert(size_t j, const gtsam::Rot2& t); - void insert(size_t j, const gtsam::Pose2& t); - void insert(size_t j, const gtsam::Rot3& t); - void insert(size_t j, const gtsam::Pose3& t); - void insert(size_t j, const gtsam::Cal3_S2& t); - void insert(size_t j, const gtsam::Cal3DS2& t); - void insert(size_t j, const gtsam::Cal3Bundler& t); - void insert(size_t j, const gtsam::EssentialMatrix& t); - void insert(size_t j, const gtsam::SimpleCamera& t); - void insert(size_t j, const gtsam::imuBias::ConstantBias& t); - void insert(size_t j, Vector t); - void insert(size_t j, Matrix t); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + 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::imuBias::ConstantBias& constant_bias); + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); - void update(size_t j, const gtsam::Point2& t); - void update(size_t j, const gtsam::Point3& t); - void update(size_t j, const gtsam::Rot2& t); - void update(size_t j, const gtsam::Pose2& t); - void update(size_t j, const gtsam::Rot3& t); - void update(size_t j, const gtsam::Pose3& t); - void update(size_t j, const gtsam::Cal3_S2& t); - void update(size_t j, const gtsam::Cal3DS2& t); - void update(size_t j, const gtsam::Cal3Bundler& t); - void update(size_t j, const gtsam::EssentialMatrix& t); - void update(size_t j, const gtsam::imuBias::ConstantBias& t); - void update(size_t j, Vector t); - void update(size_t j, Matrix t); + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + 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, Vector vector); + void update(size_t j, Matrix matrix); template T at(size_t j); @@ -1809,95 +1912,6 @@ class Values { double atDouble(size_t j) const; }; -// Actually a FastList -#include -class KeyList { - KeyList(); - KeyList(const gtsam::KeyList& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void pop_back(); - void pop_front(); - void sort(); - void remove(size_t key); - - void serialize() const; -}; - -// Actually a FastSet -class KeySet { - KeySet(); - KeySet(const gtsam::KeySet& other); - KeySet(const gtsam::KeyVector& other); - KeySet(const gtsam::KeyList& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet& other) const; - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(size_t key); - void merge(gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists - - void serialize() const; -}; - -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - -// Actually a FastMap -class KeyGroupMap { - KeyGroupMap(); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); -}; - #include class Marginals { Marginals(const gtsam::NonlinearFactorGraph& graph, @@ -1954,8 +1968,6 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { //************************************************************************* // Nonlinear optimizers //************************************************************************* - -#include #include virtual class NonlinearOptimizerParams { NonlinearOptimizerParams(); @@ -2020,6 +2032,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { void setVerbosityDL(string verbosityDL) const; }; +#include virtual class NonlinearOptimizer { gtsam::Values optimize(); gtsam::Values optimizeSafely(); @@ -2029,17 +2042,20 @@ virtual class NonlinearOptimizer { void iterate() const; }; +#include virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); }; +#include virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); double getDelta() const; }; +#include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); @@ -2100,10 +2116,10 @@ class ISAM2Params { void print(string str) const; /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); - void setRelinearizeThreshold(double relinearizeThreshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); int getRelinearizeSkip() const; void setRelinearizeSkip(int relinearizeSkip); bool isEnableRelinearization() const; @@ -2163,7 +2179,11 @@ class ISAM2 { gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; - gtsam::Value calculateEstimate(size_t key) const; + template + VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; gtsam::VectorValues getDelta() const; @@ -2251,6 +2271,17 @@ typedef gtsam::RangeFactor Ran typedef gtsam::RangeFactor RangeFactorSimpleCamera; +#include +template +virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); +}; + +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint2; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint3; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; + #include template virtual class BearingFactor : gtsam::NoiseModelFactor { @@ -2309,7 +2340,8 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { gtsam::Point2 measured() const; }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { @@ -2472,6 +2504,11 @@ class NavState { #include virtual class PreintegratedRotationParams { PreintegratedRotationParams(); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); + void setGyroscopeCovariance(Matrix cov); void setOmegaCoriolis(Vector omega); void setBodyPSensor(const gtsam::Pose3& pose); @@ -2479,13 +2516,23 @@ virtual class PreintegratedRotationParams { Matrix getGyroscopeCovariance() const; // TODO(frank): allow optional - // boost::optional getOmegaCoriolis() const; + // boost::optional getOmegaCoriolis() const; // boost::optional getBodyPSensor() const; }; #include virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); + + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); + void setAccelerometerCovariance(Matrix cov); void setIntegrationCovariance(Matrix cov); void setUse2ndOrderCoriolis(bool flag); @@ -2570,7 +2617,6 @@ virtual class CombinedImuFactor: gtsam::NonlinearFactor { class PreintegratedAhrsMeasurements { // Standard Constructor PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); // Testable @@ -2619,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ gtsam::Unit3 bRef() const; }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); void print(string s) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; @@ -2630,13 +2678,37 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ gtsam::Unit3 bRef() const; }; +#include +virtual class Scenario { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3& nTb0); +}; + +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + Vector v0, Vector a_n, + Vector omega_b); +}; + //************************************************************************* // Utilities //************************************************************************* namespace utilities { - #include + #include gtsam::KeyList createKeyList(Vector I); gtsam::KeyList createKeyList(string s, Vector I); gtsam::KeyVector createKeyVector(Vector I); @@ -2660,4 +2732,10 @@ namespace utilities { } //\namespace utilities +#include +class RedirectCout { + RedirectCout(); + string str(); +}; + } diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example deleted file mode 100755 index aa64a157d..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out index b456b0f11..ca0dc0021 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.9, Apr 1, 2016: OK. +ccolamd version 2.9, May 4, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.9, Apr 1, 2016: OK. +csymamd version 2.9, May 4, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example deleted file mode 100755 index a695d3823..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out index 559c66098..61bbe1c60 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.9, Apr 1, 2016: OK. +ccolamd version 2.9, May 4, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.9, Apr 1, 2016: OK. +csymamd version 2.9, May 4, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out b/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out deleted file mode 100644 index dd2dc4955..000000000 --- a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out +++ /dev/null @@ -1,50 +0,0 @@ -ccolamd 5-by-4 input matrix: -Column 0, with 3 entries: - row 0 - row 1 - row 4 -Column 1, with 2 entries: - row 2 - row 4 -Column 2, with 4 entries: - row 0 - row 1 - row 2 - row 3 -Column 3, with 2 entries: - row 1 - row 3 - -ccolamd version 2.7, Jan 25, 2011: OK. -ccolamd: number of dense or empty rows ignored: 0 -ccolamd: number of dense or empty columns ignored: 0 -ccolamd: number of garbage collections performed: 0 -ccolamd column ordering: -1st column: 1 -2nd column: 0 -3rd column: 3 -4th column: 2 - - -csymamd 5-by-5 input matrix: -Entries in strictly lower triangular part: -Column 0, with 1 entries: - row 1 -Column 1, with 2 entries: - row 2 - row 3 -Column 2, with 0 entries: -Column 3, with 1 entries: - row 4 -Column 4, with 0 entries: - -csymamd version 2.7, Jan 25, 2011: OK. -csymamd: number of dense or empty rows ignored: 0 -csymamd: number of dense or empty columns ignored: 0 -csymamd: number of garbage collections performed: 0 -csymamd column ordering: -1st row/column: 0 -2nd row/column: 2 -3rd row/column: 1 -4th row/column: 3 -5th row/column: 4 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out b/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out deleted file mode 100644 index fc87f5474..000000000 --- a/gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out +++ /dev/null @@ -1,50 +0,0 @@ -ccolamd 5-by-4 input matrix: -Column 0, with 3 entries: - row 0 - row 1 - row 4 -Column 1, with 2 entries: - row 2 - row 4 -Column 2, with 4 entries: - row 0 - row 1 - row 2 - row 3 -Column 3, with 2 entries: - row 1 - row 3 - -ccolamd version 2.7, Jan 25, 2011: OK. -ccolamd: number of dense or empty rows ignored: 0 -ccolamd: number of dense or empty columns ignored: 0 -ccolamd: number of garbage collections performed: 0 -ccolamd_l column ordering: -1st column: 1 -2nd column: 0 -3rd column: 3 -4th column: 2 - - -csymamd_l 5-by-5 input matrix: -Entries in strictly lower triangular part: -Column 0, with 1 entries: - row 1 -Column 1, with 2 entries: - row 2 - row 3 -Column 2, with 0 entries: -Column 3, with 1 entries: - row 4 -Column 4, with 0 entries: - -csymamd version 2.7, Jan 25, 2011: OK. -csymamd: number of dense or empty rows ignored: 0 -csymamd: number of dense or empty columns ignored: 0 -csymamd: number of garbage collections performed: 0 -csymamd_l column ordering: -1st row/column: 0 -2nd row/column: 2 -3rd row/column: 1 -4th row/column: 3 -5th row/column: 4 diff --git a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog index 85f375c7a..0e3eab497 100644 --- a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog +++ b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog @@ -1,3 +1,7 @@ +May 4, 2016: version 2.9.6 + + * minor changes to Makefile + Apr 1, 2016: version 2.9.5 * licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt diff --git a/gtsam/3rdparty/CCOLAMD/Doc/License.txt b/gtsam/3rdparty/CCOLAMD/Doc/License.txt index 089509a6b..66bb848dc 100644 --- a/gtsam/3rdparty/CCOLAMD/Doc/License.txt +++ b/gtsam/3rdparty/CCOLAMD/Doc/License.txt @@ -6,16 +6,28 @@ http://www.suitesparse.com -------------------------------------------------------------------------------- -CCOLAMD is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. +CCOLAMD license: BSD 3-clause: -CCOLAMD is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the organizations to which the authors are + affiliated, nor the names of its contributors may be used to endorse + or promote products derived from this software without specific prior + written permission. -You should have received a copy of the GNU Lesser General Public -License along with this Module; if not, write to the Free Software -Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + DAMAGE. diff --git a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h index b4ee829be..8c2129ef3 100644 --- a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h +++ b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h @@ -41,11 +41,11 @@ extern "C" { * #endif */ -#define CCOLAMD_DATE "Apr 1, 2016" +#define CCOLAMD_DATE "May 4, 2016" #define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub)) #define CCOLAMD_MAIN_VERSION 2 #define CCOLAMD_SUB_VERSION 9 -#define CCOLAMD_SUBSUB_VERSION 5 +#define CCOLAMD_SUBSUB_VERSION 6 #define CCOLAMD_VERSION \ CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) diff --git a/gtsam/3rdparty/CCOLAMD/Lib/Makefile b/gtsam/3rdparty/CCOLAMD/Lib/Makefile index c2352c90e..52c52eb9e 100644 --- a/gtsam/3rdparty/CCOLAMD/Lib/Makefile +++ b/gtsam/3rdparty/CCOLAMD/Lib/Makefile @@ -3,7 +3,7 @@ #------------------------------------------------------------------------------- LIBRARY = libccolamd -VERSION = 2.9.5 +VERSION = 2.9.6 SO_VERSION = 2 default: library @@ -32,6 +32,8 @@ ccolamd_l.o: $(SRC) $(INC) $(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o # creates libccolamd.a, a C-callable CCOLAMD library +static: $(AR_TARGET) + $(AR_TARGET): $(OBJ) $(ARCHIVE) $@ $^ - $(RANLIB) $@ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a b/gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a deleted file mode 100644 index dc4502e9e..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/libccolamd.a and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Makefile b/gtsam/3rdparty/CCOLAMD/Makefile index f04181d60..ecebb8b92 100644 --- a/gtsam/3rdparty/CCOLAMD/Makefile +++ b/gtsam/3rdparty/CCOLAMD/Makefile @@ -20,6 +20,10 @@ all: library: ( cd Lib ; $(MAKE) ) +# compile the static libraries only +static: + ( cd Lib ; $(MAKE) static ) + # remove object files, but keep the compiled programs and library archives clean: ( cd Lib ; $(MAKE) clean ) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 9a08e3ea8..d5e8da2f0 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -1560,8 +1560,9 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ Int *dead_cols ; Int set1 ; Int set2 ; +#ifndef NDEBUG Int cs ; - +#endif int ok ; #ifndef NDEBUG @@ -1909,8 +1910,10 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ p [k] = col ; ASSERT (A [col] == EMPTY) ; - cs = CMEMBER (col) ; +#ifndef NDEBUG + cs = CMEMBER (col) ; ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; +#endif A [col] = k ; k++ ; @@ -1926,11 +1929,11 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ if (A [col] == EMPTY) { k = Col [col].shared2.order ; - cs = CMEMBER (col) ; #ifndef NDEBUG + cs = CMEMBER (col) ; dead_cols [cs]-- ; -#endif ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; +#endif DEBUG1 (("ccolamd output ordering: k "ID" col "ID " (dense or null col)\n", k, col)) ; p [k] = col ; diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c deleted file mode 100644 index e470804a6..000000000 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c +++ /dev/null @@ -1,28 +0,0 @@ -/* ========================================================================== */ -/* === ccolamd_global.c ===================================================== */ -/* ========================================================================== */ - -/* ---------------------------------------------------------------------------- - * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, - * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse - * -------------------------------------------------------------------------- */ - -/* Global variables for CCOLAMD */ - -#ifndef NPRINT -#ifdef MATLAB_MEX_FILE -#include -#include -typedef uint16_t char16_t; -#include "mex.h" -int (*ccolamd_printf) (const char *, ...) = mexPrintf ; -#else -#include -int (*ccolamd_printf) (const char *, ...) = printf ; -#endif -#else -int (*ccolamd_printf) (const char *, ...) = ((void *) 0) ; -#endif - diff --git a/gtsam/3rdparty/SuiteSparse_config/Makefile b/gtsam/3rdparty/SuiteSparse_config/Makefile index 96db5772f..aa858aeab 100644 --- a/gtsam/3rdparty/SuiteSparse_config/Makefile +++ b/gtsam/3rdparty/SuiteSparse_config/Makefile @@ -7,7 +7,7 @@ export SUITESPARSE # version of SuiteSparse_config is also version of SuiteSparse meta-package LIBRARY = libsuitesparseconfig -VERSION = 4.5.2 +VERSION = 4.5.6 SO_VERSION = 4 default: library @@ -27,6 +27,8 @@ OBJ = SuiteSparse_config.o SuiteSparse_config.o: SuiteSparse_config.c SuiteSparse_config.h $(CC) $(CF) -c SuiteSparse_config.c +static: $(AR_TARGET) + $(AR_TARGET): $(OBJ) $(ARCHIVE) $(AR_TARGET) SuiteSparse_config.o $(RANLIB) $(AR_TARGET) diff --git a/gtsam/3rdparty/SuiteSparse_config/README.txt b/gtsam/3rdparty/SuiteSparse_config/README.txt index a76a5fab6..8129f5a04 100644 --- a/gtsam/3rdparty/SuiteSparse_config/README.txt +++ b/gtsam/3rdparty/SuiteSparse_config/README.txt @@ -1,4 +1,4 @@ -SuiteSparse_config, 2016, Timothy A. Davis, http://www.suitesparse.com +SuiteSparse_config, 2017, Timothy A. Davis, http://www.suitesparse.com (formerly the UFconfig package) This directory contains a default SuiteSparse_config.mk file. It tries to diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h index 49296fc5a..7d4de65d3 100644 --- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h @@ -184,24 +184,24 @@ int SuiteSparse_divcomplex * * SuiteSparse contains the following packages: * - * SuiteSparse_config version 4.5.2 (version always the same as SuiteSparse) - * AMD version 2.4.5 - * BTF version 1.2.5 - * CAMD version 2.4.5 - * CCOLAMD version 2.9.5 - * CHOLMOD version 3.0.10 - * COLAMD version 2.9.5 - * CSparse version 3.1.8 - * CXSparse version 3.1.8 - * GPUQREngine version 1.0.4 - * KLU version 1.3.7 - * LDL version 2.2.5 - * RBio version 2.2.5 - * SPQR version 2.0.6 - * SuiteSparse_GPURuntime version 1.0.4 - * UMFPACK version 5.7.5 + * SuiteSparse_config version 4.5.6 (version always the same as SuiteSparse) + * AMD version 2.4.6 + * BTF version 1.2.6 + * CAMD version 2.4.6 + * CCOLAMD version 2.9.6 + * CHOLMOD version 3.0.11 + * COLAMD version 2.9.6 + * CSparse version 3.1.9 + * CXSparse version 3.1.9 + * GPUQREngine version 1.0.5 + * KLU version 1.3.8 + * LDL version 2.2.6 + * RBio version 2.2.6 + * SPQR version 2.0.8 + * SuiteSparse_GPURuntime version 1.0.5 + * UMFPACK version 5.7.6 * MATLAB_Tools various packages & M-files - * xerbla version 1.0.2 + * xerbla version 1.0.3 * * Other package dependencies: * BLAS required by CHOLMOD and UMFPACK @@ -211,7 +211,6 @@ int SuiteSparse_divcomplex * they are compiled with GPU acceleration. */ - int SuiteSparse_version /* returns SUITESPARSE_VERSION */ ( /* output, not defined on input. Not used if NULL. Returns @@ -234,11 +233,11 @@ int SuiteSparse_version /* returns SUITESPARSE_VERSION */ */ #define SUITESPARSE_HAS_VERSION_FUNCTION -#define SUITESPARSE_DATE "Apr 1, 2016" +#define SUITESPARSE_DATE "Oct 3, 2017" #define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) #define SUITESPARSE_MAIN_VERSION 4 #define SUITESPARSE_SUB_VERSION 5 -#define SUITESPARSE_SUBSUB_VERSION 2 +#define SUITESPARSE_SUBSUB_VERSION 6 #define SUITESPARSE_VERSION \ SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk index 40ad6b9af..2c13a6010 100644 --- a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk @@ -5,7 +5,7 @@ # This file contains all configuration settings for all packages in SuiteSparse, # except for CSparse (which is stand-alone) and the packages in MATLAB_Tools. -SUITESPARSE_VERSION = 4.5.2 +SUITESPARSE_VERSION = 4.5.6 #=============================================================================== # Options you can change without editing this file: @@ -115,6 +115,7 @@ SUITESPARSE_VERSION = 4.5.2 CC = icc -D_GNU_SOURCE CXX = $(CC) CFOPENMP = -qopenmp -I$(MKLROOT)/include + LDFLAGS += -openmp endif ifneq ($(shell which ifort 2>/dev/null),) # use the Intel ifort compiler for Fortran codes @@ -123,7 +124,7 @@ SUITESPARSE_VERSION = 4.5.2 endif #--------------------------------------------------------------------------- - # code formatting (for Tcov only) + # code formatting (for Tcov on Linux only) #--------------------------------------------------------------------------- PRETTY ?= grep -v "^\#" | indent -bl -nce -bli0 -i4 -sob -l120 @@ -224,7 +225,6 @@ SUITESPARSE_VERSION = 4.5.2 CUDA_INC = -I$(CUDA_INC_PATH) NVCC = $(CUDA_PATH)/bin/nvcc NVCCFLAGS = -Xcompiler -fPIC -O3 \ - -gencode=arch=compute_20,code=sm_20 \ -gencode=arch=compute_30,code=sm_30 \ -gencode=arch=compute_35,code=sm_35 \ -gencode=arch=compute_50,code=sm_50 \ @@ -305,8 +305,9 @@ SUITESPARSE_VERSION = 4.5.2 SPQR_CONFIG ?= $(GPU_CONFIG) - # to compile with Intel's TBB, use TBB=-ltbb SPQR_CONFIG=-DHAVE_TBB + # to compile with Intel's TBB, use TBB=-ltbb -DSPQR_CONFIG=-DHAVE_TBB TBB ?= + # TBB = -ltbb -DSPQR_CONFIG=-DHAVE_TBB # TODO: this *mk file should auto-detect the presence of Intel's TBB, # and set the compiler flags accordingly. diff --git a/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile index 420c50e88..db68a2ea8 100644 --- a/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile +++ b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile @@ -4,7 +4,7 @@ USE_FORTRAN = 0 # USE_FORTRAN = 1 -VERSION = 1.0.2 +VERSION = 1.0.3 SO_VERSION = 1 default: library @@ -35,6 +35,8 @@ ccode: all fortran: all +static: $(AR_TARGET) + $(AR_TARGET): $(DEPENDS) $(COMPILE) $(ARCHIVE) $(AR_TARGET) xerbla.o diff --git a/gtsam/3rdparty/metis/GKlib/csr.c b/gtsam/3rdparty/metis/GKlib/csr.c index a19d793bd..4f6213c49 100644 --- a/gtsam/3rdparty/metis/GKlib/csr.c +++ b/gtsam/3rdparty/metis/GKlib/csr.c @@ -1323,55 +1323,52 @@ void gk_csr_Normalize(gk_csr_t *mat, int what, int norm) ssize_t *ptr; float *val, sum; - if (what&GK_CSR_ROW && mat->rowval) { - n = mat->nrows; + if (what & GK_CSR_ROW && mat->rowval) { + n = mat->nrows; ptr = mat->rowptr; val = mat->rowval; - #pragma omp parallel if (ptr[n] > OMPMINOPS) +#pragma omp parallel if (ptr[n] > OMPMINOPS) { - #pragma omp for private(j,sum) schedule(static) - for (i=0; i 0 */ +#pragma omp for private(j, sum) schedule(static) + for (i = 0; i < n; i++) { + for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++) { + if (norm == 2) + sum += val[j] * val[j]; + else if (norm == 1) + sum += val[j]; /* assume val[j] > 0 */ } if (sum > 0) { - if (norm == 2) - sum=1.0/sqrt(sum); - else if (norm == 1) - sum=1.0/sum; - for (j=ptr[i]; jcolval) { - n = mat->ncols; + if (what & GK_CSR_COL && mat->colval) { + n = mat->ncols; ptr = mat->colptr; val = mat->colval; - #pragma omp parallel if (ptr[n] > OMPMINOPS) +#pragma omp parallel if (ptr[n] > OMPMINOPS) { - #pragma omp for private(j,sum) schedule(static) - for (i=0; i 0) { - if (norm == 2) - sum=1.0/sqrt(sum); - else if (norm == 1) - sum=1.0/sum; - for (j=ptr[i]; jncon == 1) { /* return right away if the balance is OK */ - if (iabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs) + if (fabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs) return; if (graph->nbnd > 0) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 8c1d8bb43..3875b6a19 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -25,7 +25,6 @@ add_subdirectory(3rdparty) set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config/SuiteSparse_config.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure @@ -131,6 +130,11 @@ else() DEFINE_SYMBOL GTSAM_EXPORTS RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") endif() + if (APPLE) + set_target_properties(gtsam PROPERTIES + INSTALL_NAME_DIR + "${CMAKE_INSTALL_PREFIX}/lib") + endif() install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) @@ -138,7 +142,7 @@ endif() # make sure that ccolamd compiles even in face of warnings if(WIN32) - set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "/w") + set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w") else() set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error") endif() @@ -165,3 +169,4 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Wrap wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") endif () + diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 17783c5b9..93a7d0db5 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include // operator typeid namespace gtsam { @@ -190,7 +190,7 @@ public: } /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues -#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(GenericValue) +#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue) }; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d74e1c122..0c37b405e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -32,6 +32,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 463b5f5d9..87ead88f0 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -138,17 +138,27 @@ public: return ProductLieGroup(g,h); } static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); - G g = traits::Expmap(v.template head()); - H h = traits::Expmap(v.template tail()); + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); + H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); + if (Hv) { + Hv->setZero(); + Hv->template topLeftCorner() = D_g_first; + Hv->template bottomRightCorner() = D_h_second; + } return ProductLieGroup(g,h); } static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); - typename traits::TangentVector v1 = traits::Logmap(p.first); - typename traits::TangentVector v2 = traits::Logmap(p.second); + Jacobian1 D_g_first; Jacobian2 D_h_second; + typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); + typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); TangentVector v; v << v1, v2; + if (Hp) { + Hp->setZero(); + Hp->template topLeftCorner() = D_g_first; + Hp->template bottomRightCorner() = D_h_second; + } return v; } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index da3a9a8b8..53a8912f6 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include namespace boost { namespace serialization { diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 8f17e4c6e..6dc9800ca 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 5456ae7f5..43644b5c4 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -138,14 +138,14 @@ struct VectorSpaceImpl { } static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + ChartJacobian H2 = boost::none) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v2); return v1 + v2; } static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + ChartJacobian H2 = boost::none) { if (H1) *H1 = - Eye(v1); if (H2) *H2 = Eye(v2); return v2 - v1; diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 7bc5949cc..31e4b8244 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -111,8 +111,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { return true; assert(ABC.cols() == ABC.rows()); - const Eigen::DenseIndex n = ABC.rows() - topleft; - assert(n >= 0 && nFrontal <= size_t(n)); + assert(ABC.rows() >= topleft); + const size_t n = static_cast(ABC.rows() - topleft); + assert(nFrontal <= size_t(n)); // Create views on blocks auto A = ABC.block(topleft, topleft, nFrontal, nFrontal); @@ -144,8 +145,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { // Check last diagonal element - Eigen does not check it if (nFrontal >= 2) { int exp2, exp1; - (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2); - (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1); + // NOTE(gareth): R is already the size of A, so we don't need to add topleft here. + (void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2); + (void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1); return (exp2 - exp1 < underconstrainedExponentDifference); } else if (nFrontal == 1) { int exp1; diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index f9c424e95..68632addc 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index dcb0425b7..2fa6e33c6 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -298,7 +298,7 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* /** * Compute numerical derivative in argument 1 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 n-dimensional first argument value * @param x2 second argument value * @param x3 third argument value @@ -317,9 +317,15 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( return numericalDerivative11(boost::bind(h, _1, x2, x3, x4), x1, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative41(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + /** * Compute numerical derivative in argument 2 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 first argument value * @param x2 n-dimensional second argument value * @param x3 third argument value @@ -338,9 +344,15 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative42(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + /** * Compute numerical derivative in argument 3 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 first argument value * @param x2 second argument value * @param x3 n-dimensional third argument value @@ -359,9 +371,15 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( return numericalDerivative11(boost::bind(h, x1, x2, _1, x4), x3, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative43(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + /** * Compute numerical derivative in argument 4 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 first argument value * @param x2 second argument value * @param x3 third argument value @@ -380,6 +398,152 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( return numericalDerivative11(boost::bind(h, x1, x2, x3, _1), x4, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative44(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + +/** + * Compute numerical derivative in argument 1 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative51( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3, x4, x5), x1, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative51(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 2 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative52( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4, x5), x2, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative52(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 3 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative53( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1, x4, x5), x3, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative53(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 4 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative54( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, x3, _1, x5), x4, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative54(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 5 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative55( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, x3, x4, _1), x5, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative55(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index c55e5067b..4ef09beb1 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -24,6 +24,9 @@ #include +#include + + // whether to print the serialized text to stdout const bool verbose = false; @@ -142,4 +145,3 @@ bool equalsDereferencedBinary(const T& input = T()) { } // \namespace serializationTestHelpers } // \namespace gtsam - diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index 1db28bcc8..d863eaba3 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file testSerializationBase.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 27fe2f197..b1e003864 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 81463ac06..4009a1921 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -23,18 +23,11 @@ namespace gtsam { /** - * @brief Calibration of a camera with radial distortion + * @brief Calibration of a camera with radial distortion that also supports + * Lie-group behaviors for optimization. + * \sa Cal3DS2_Base * @addtogroup geometry * \nosubgrouping - * - * Uses same distortionmodel as OpenCV, with - * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html - * but using only k1,k2,p1, and p2 coefficients. - * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - * rr = Pn.x^2 + Pn.y^2 - * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - * pi = K*pn */ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index cfbdde07c..4da5d1360 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -32,9 +32,9 @@ namespace gtsam { * but using only k1,k2,p1, and p2 coefficients. * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] * rr = Pn.x^2 + Pn.y^2 - * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - * pi = K*pn + * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ; + * p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y ] + * pi = K*Pn */ class GTSAM_EXPORT Cal3DS2_Base { diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp new file mode 100644 index 000000000..22966ee37 --- /dev/null +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3_S2Stereo.cpp + * @brief The most common 5DOF 3D->2D calibration + Stereo baseline + * @author Chris Beall + */ + +#include + +#include + +namespace gtsam { +using namespace std; + +/* ************************************************************************* */ +void Cal3_S2Stereo::print(const std::string& s) const { + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; + } + +/* ************************************************************************* */ +bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 365a6c40e..db49448ec 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { @@ -62,16 +63,10 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + void print(const std::string& s = "") const; /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { - if (fabs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); - } + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 026becebe..3c9fb1563 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -142,8 +142,11 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, Matrix23 Dpc_rot; Matrix2 Dpc_point; const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0, - Dpose ? &Dpc_point : 0); - + Dpoint ? &Dpc_point : 0); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (pc.unitVector().z() <= 0) + throw CheiralityException(); +#endif // camera to normalized image coordinate Matrix2 Dpn_pc; const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); @@ -161,8 +164,12 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, return pn; } /* ************************************************************************* */ -Point3 PinholeBase::backproject_from_camera(const Point2& p, - const double depth) { +Point3 PinholeBase::BackprojectFromCamera(const Point2& p, + const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) { + if (Dpoint) + *Dpoint << depth, 0, 0, depth, 0, 0; + if (Ddepth) + *Ddepth << p.x(), p.y(), 1; return Point3(p.x() * depth, p.y() * depth, depth); } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f1fa509c1..db9caf13b 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -55,6 +55,7 @@ public: * and this typedef informs those classes what "project" returns. */ typedef Point2 Measurement; + typedef Point2Vector MeasurementVector; private: @@ -201,7 +202,9 @@ public: OptionalJacobian<2, 2> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - static Point3 backproject_from_camera(const Point2& p, const double depth); + static Point3 BackprojectFromCamera(const Point2& p, const double depth, + OptionalJacobian<3, 2> Dpoint = boost::none, + OptionalJacobian<3, 1> Ddepth = boost::none); /// @} /// @name Advanced interface @@ -227,6 +230,9 @@ private: ar & BOOST_SERIALIZATION_NVP(pose_); } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; // end of class PinholeBase @@ -337,8 +343,28 @@ public: boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& pn, double depth) const { - return pose().transform_from(backproject_from_camera(pn, depth)); + Point3 backproject(const Point2& pn, double depth, + OptionalJacobian<3, 6> Dresult_dpose = boost::none, + OptionalJacobian<3, 2> Dresult_dp = boost::none, + OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const { + + Matrix32 Dpoint_dpn; + Matrix31 Dpoint_ddepth; + const Point3 point = BackprojectFromCamera(pn, depth, + Dresult_dp ? &Dpoint_dpn : 0, + Dresult_ddepth ? &Dpoint_ddepth : 0); + + Matrix33 Dresult_dpoint; + const Point3 result = pose().transform_from(point, Dresult_dpose, + (Dresult_ddepth || + Dresult_dp) ? &Dresult_dpoint : 0); + + if (Dresult_dp) + *Dresult_dp = Dresult_dpoint * Dpoint_dpn; + if (Dresult_ddepth) + *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; + + return result; } /** @@ -390,6 +416,9 @@ private: } /// @} + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // manifold traits @@ -404,4 +433,3 @@ template struct Range : HasRange {}; } // namespace gtsam - diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b322a40ab..709172c5b 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -31,7 +31,7 @@ namespace gtsam { * @brief A set of cameras, all with their own calibration */ template -class CameraSet: public std::vector { +class CameraSet : public std::vector > { protected: @@ -40,13 +40,14 @@ protected: * The order is kept the same as the keys that we use to create the factor. */ typedef typename CAMERA::Measurement Z; + typedef typename CAMERA::MeasurementVector ZVector; static const int D = traits::dimension; ///< Camera dimension static const int ZDim = traits::dimension; ///< Measurement dimension /// Make a vector of re-projection errors - static Vector ErrorVector(const std::vector& predicted, - const std::vector& measured) { + static Vector ErrorVector(const ZVector& predicted, + const ZVector& measured) { // Check size size_t m = predicted.size(); @@ -56,7 +57,11 @@ protected: // Project and fill error vector Vector b(ZDim * m); for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - b.segment(row) = traits::Local(measured[i], predicted[i]); + Vector bi = traits::Local(measured[i], predicted[i]); + if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; } return b; } @@ -65,7 +70,7 @@ public: /// Definitions for blocks of F typedef Eigen::Matrix MatrixZD; - typedef std::vector FBlocks; + typedef std::vector > FBlocks; /** * print @@ -98,7 +103,7 @@ public: * throws CheiralityException */ template - std::vector project2(const POINT& point, // + ZVector project2(const POINT& point, // boost::optional Fs = boost::none, // boost::optional E = boost::none) const { @@ -106,7 +111,7 @@ public: // Allocate result size_t m = this->size(); - std::vector z; + ZVector z; z.reserve(m); // Allocate derivatives @@ -127,7 +132,7 @@ public: /// Calculate vector [project2(point)-z] of re-projection errors template - Vector reprojectionError(const POINT& point, const std::vector& measured, + Vector reprojectionError(const POINT& point, const ZVector& measured, boost::optional Fs = boost::none, // boost::optional E = boost::none) const { return ErrorVector(project2(point, Fs, E), measured); @@ -311,6 +316,9 @@ private: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & (*this); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; template diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index a5707ce89..beadba929 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -39,6 +39,7 @@ public: * and this typedef informs those classes what "project" returns. */ typedef Point2 Measurement; + typedef Point2Vector MeasurementVector; private: @@ -323,6 +324,8 @@ private: ar & BOOST_SERIALIZATION_NVP(K_); } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // manifold traits diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 43ba78ea2..f3b99b2fb 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -85,25 +85,9 @@ public: return pn; } - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinates - */ - Point2 project(const Point3& pw) const { - const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates - return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates - } - - /** project a point from world coordinate to the image - * @param pw is a point at infinity in the world coordinates - */ - Point2 project(const Unit3& pw) const { - const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame - const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates - return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates - } /** Templated projection of a point (possibly at infinity) from world coordinate to the image - * @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates + * @param pw is a 3D point or a Unit3 (point at infinity) in world coordinates * @param Dpose is the Jacobian w.r.t. pose3 * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration @@ -131,25 +115,51 @@ public: } /// project a 3D point from world coordinates into the image - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { return _project(pw, Dpose, Dpoint, Dcal); } /// project a point at infinity from world coordinates into the image - Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { return _project(pw, Dpose, Dpoint, Dcal); } /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = calibration().calibrate(p); - return pose().transform_from(backproject_from_camera(pn, depth)); + Point3 backproject(const Point2& p, double depth, + OptionalJacobian<3, 6> Dresult_dpose = boost::none, + OptionalJacobian<3, 2> Dresult_dp = boost::none, + OptionalJacobian<3, 1> Dresult_ddepth = boost::none, + OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const { + typedef Eigen::Matrix Matrix2K; + Matrix2K Dpn_dcal; + Matrix22 Dpn_dp; + const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0, + Dresult_dp ? &Dpn_dp : 0); + Matrix32 Dpoint_dpn; + Matrix31 Dpoint_ddepth; + const Point3 point = BackprojectFromCamera(pn, depth, + (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, + Dresult_ddepth ? &Dpoint_ddepth : 0); + Matrix33 Dresult_dpoint; + const Point3 result = pose().transform_from(point, Dresult_dpose, + (Dresult_ddepth || + Dresult_dp || + Dresult_dcal) ? &Dresult_dpoint : 0); + if (Dresult_dcal) + *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK) + if (Dresult_dp) + *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2) + if (Dresult_ddepth) + *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1) + + return result; } + /// backproject a 2-dimensional point to a 3-dimensional point at infinity Unit3 backprojectPointAtInfinity(const Point2& p) const { const Point2 pn = calibration().calibrate(p); @@ -210,7 +220,9 @@ private: & boost::serialization::make_nvp("PinholeBase", boost::serialization::base_object(*this)); } - + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholeBaseK @@ -302,6 +314,11 @@ public: Base(v), K_(new CALIBRATION(K)) { } + // Init from Pose3 and calibration + PinholePose(const Pose3 &pose, const Vector &K) : + Base(pose), K_(new CALIBRATION(K)) { + } + /// @} /// @name Testable /// @{ @@ -407,6 +424,8 @@ private: ar & BOOST_SERIALIZATION_NVP(K_); } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholePose diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 65047df41..7869704ca 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -58,7 +58,7 @@ public: /// triangulateSafe TriangulationResult triangulateSafe( - const std::vector& measured, + const typename CAMERA::MeasurementVector& measured, const TriangulationParameters& params) const { return gtsam::triangulateSafe(*this, measured, params); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 2152a7c39..74b9a2bec 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -17,6 +17,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index fb250df6d..3657ebf05 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -164,7 +164,7 @@ typedef std::pair Point2Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); // For MATLAB wrapper -typedef std::vector Point2Vector; +typedef std::vector > Point2Vector; /// multiply with scalar inline Point2 operator*(double s, const Point2& p) { diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 091906d5f..8df5f5607 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -16,6 +16,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 25cd661e8..50e487e75 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -378,6 +378,16 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } } +/* ************************************************************************* */ +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)); + } + return bearing(pose.translation(), H1, boost::none); +} + /* ************************************************************************* */ boost::optional Pose3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ee1206119..3229cbbbe 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -282,6 +282,15 @@ public: Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, OptionalJacobian<2, 3> H2 = boost::none) const; + /** + * Calculate bearing to another pose + * @param other 3D location and orientation of other body. The orientation + * information is ignored. + * @return bearing (Unit3) + */ + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none, + OptionalJacobian<2, 6> H2 = boost::none) const; + /// @} /// @name Advanced Interface /// @{ @@ -318,6 +327,9 @@ public: } /// @} + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; // Pose3 class @@ -353,6 +365,9 @@ struct traits : public internal::LieGroup {}; template <> struct Bearing : HasBearing {}; +template<> +struct Bearing : HasBearing {}; + template struct Range : HasRange {}; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 0ab4a5ee6..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,6 +21,7 @@ #include #include // Logmap/Expmap derivatives #include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9780cb49a..4cabe7140 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace std; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index bb0278953..263301122 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -118,7 +118,7 @@ namespace gtsam { * @param q The quaternion */ Rot3(const Quaternion& q); - Rot3(double x, double y, double z, double w) : Rot3(Quaternion(x, y, z, w)) {} + Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {} /// Random, generates a random axis, then random angle \in [-p,pi] static Rot3 Random(boost::mt19937 & rng); @@ -508,6 +508,9 @@ namespace gtsam { #endif } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; /** diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bd111d9b1..07c03ab49 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -116,6 +117,12 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } +/* ************************************************************************* */ +void SO3::print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + +/* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 3152fa2fb..53f2c2130 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { @@ -67,9 +68,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const { - std::cout << s << *this << std::endl; - } + void print(const std::string& s) const; bool equals(const SO3 & R, double tol) const { return equal_with_abs_tol(*this, R, tol); diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 5c6646454..c34d4d8f3 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -39,7 +39,10 @@ namespace gtsam { const Point3 q = leftCamPose_.transform_to(point); - if ( q.z() <= 0 ) throw StereoCheiralityException(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw StereoCheiralityException(); +#endif // get calibration const Cal3_S2Stereo& K = *K_; diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index ac32be7ae..db4f74026 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -43,6 +43,7 @@ public: * and this typedef informs those classes what "project" returns. */ typedef StereoPoint2 Measurement; + typedef StereoPoint2Vector MeasurementVector; private: Pose3 leftCamPose_; diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 961cc041b..dd9bc9dbd 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -159,6 +159,8 @@ private: }; +typedef std::vector StereoPoint2Vector; + template<> struct traits : public internal::VectorSpace {}; diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp old mode 100644 new mode 100755 index dacb5c3fd..08f14c829 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,7 +21,7 @@ #include #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB #ifdef __clang__ # pragma clang diagnostic push @@ -36,13 +36,14 @@ #include #include #include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; Unit3 direction; @@ -54,7 +55,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { /* ************************************************************************* */ Unit3 Unit3::Random(boost::mt19937 & rng) { - // TODO allow any engine without including all of boost :-( + // TODO(dellaert): allow any engine without including all of boost :-( boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). @@ -64,69 +65,76 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return Unit3(d[0], d[1], d[2]); } +/* ************************************************************************* */ +// Get the axis of rotation with the minimum projected length of the point +static Point3 CalculateBestAxis(const Point3& n) { + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + return Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + return Point3(0.0, 1.0, 0.0); + } else { + return Point3(0, 0, 1); + } +} + /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I - // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or - // there is still a latent bug to watch out for. + // 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_); #endif - // Return cached basis if available and the Jacobian isn't needed. - if (B_ && !H) { - return *B_; - } - - // Return cached basis and derivatives if available. - if (B_ && H && H_B_) { - *H = *H_B_; - return *B_; - } - - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); - if ((mx <= my) && (mx <= mz)) { - axis = Point3(1.0, 0.0, 0.0); - } else if ((my <= mx) && (my <= mz)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - B_.reset(Matrix32()); - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + const bool cachedBasis = static_cast(B_); + const bool cachedJacobian = static_cast(H_B_); if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + if (!cachedJacobian) { + // Compute Jacobian. Recomputes B_ + Matrix32 B; + Matrix62 jacobian; + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + B.col(0) = normalize(B1, &H_b1_B1); + + // Get the second basis vector b2, which is orthogonal to n and b1. + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); + + // Chain rule tomfoolery to compute the jacobian. + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the result and jacobian + H_B_.reset(jacobian); + B_.reset(B); + } + + // Return cached jacobian, possibly computed just above *H = *H_B_; } + if (!cachedBasis) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + return *B_; } @@ -161,7 +169,8 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { +double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p, + OptionalJacobian<1, 2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; Point3 pn = point3(H_p ? &H_pn_p : nullptr); @@ -185,7 +194,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 const Vector2 xi = basis().transpose() * q.p_; if (H_q) { @@ -195,7 +204,8 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { } /* ************************************************************************* */ -Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { +Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, + OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); @@ -230,7 +240,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { +double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { Matrix2 H_xi_q; const Vector2 xi = error(q, H ? &H_xi_q : nullptr); const double theta = xi.norm(); @@ -277,4 +287,4 @@ Vector2 Unit3::localCoordinates(const Unit3& other) const { } /* ************************************************************************* */ -} +} // namespace gtsam diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index cd05af519..493c7d00a 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -208,6 +208,8 @@ private: /// @} +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // Define GTSAM traits diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index e629eb3c6..33a88db1f 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -113,7 +113,7 @@ static Point2 Project2(const Unit3& point) { return PinholeBase::Project(point); } -Unit3 pointAtInfinity(0, 0, 1000); +Unit3 pointAtInfinity(0, 0, -1000); TEST( CalibratedCamera, DProjectInfinity) { Matrix test1; CalibratedCamera::Project(pointAtInfinity, test1); @@ -127,6 +127,7 @@ static Point2 project2(const CalibratedCamera& camera, const Point3& point) { return camera.project(point); } + TEST( CalibratedCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; @@ -173,7 +174,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2_infinity) { - static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); static const CalibratedCamera camera(pose); Matrix Dpose, Dpoint; camera.project2(pointAtInfinity, Dpose, Dpoint); @@ -183,7 +184,48 @@ TEST( CalibratedCamera, Dproject_point_pose2_infinity) CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point, + const double& depth) { + return camera.BackprojectFromCamera(point, depth); +} +TEST( CalibratedCamera, DBackprojectFromCamera) +{ + static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose); + static const double depth = 5.4; + static const Point2 point(10.1, 50.3); + Matrix Dpoint, Ddepth; + camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth); + Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth); + Matrix numerical_depth = numericalDerivative33(BackprojectFromCamera, camera, point, depth); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_depth, Ddepth, 1e-7)); +} + + +static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) { + return CalibratedCamera(pose).backproject(point, depth); +} +TEST( PinholePose, Dbackproject) +{ + Matrix36 Dpose; + Matrix31 Ddepth; + Matrix32 Dpoint; + const Point2 point(-100, 100); + const double depth(10); + static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose); + camera.backproject(point, depth, Dpose, Dpoint, Ddepth); + Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth); + Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth); + Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth); + + EXPECT(assert_equal(expectedDpose, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 01f784ae0..560206b9f 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -32,7 +32,7 @@ using namespace gtsam; TEST(CameraSet, Pinhole) { typedef PinholeCamera Camera; typedef CameraSet Set; - typedef vector ZZ; + typedef Point2Vector ZZ; Set set; Camera camera; set.push_back(camera); @@ -135,8 +135,8 @@ TEST(CameraSet, Pinhole) { /* ************************************************************************* */ #include TEST(CameraSet, Stereo) { - typedef vector ZZ; CameraSet set; + typedef StereoCamera::MeasurementVector ZZ; StereoCamera camera; set.push_back(camera); set.push_back(camera); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index ecbb92061..05d48f4cc 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -191,7 +191,7 @@ static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, /* ************************************************************************* */ TEST( PinholePose, DprojectAtInfinity2) { - Unit3 pointAtInfinity(0,0,1000); + Unit3 pointAtInfinity(0,0,-1000); Matrix Dpose, Dpoint; Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K); @@ -201,6 +201,32 @@ TEST( PinholePose, DprojectAtInfinity2) EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); } + +static Point3 backproject(const Pose3& pose, const Cal3_S2& cal, + const Point2& p, const double& depth) { + return Camera(pose, cal.vector()).backproject(p, depth); +} + +TEST( PinholePose, Dbackproject) +{ + Matrix36 Dpose; + Matrix31 Ddepth; + Matrix32 Dpoint; + Matrix Dcal; + const Point2 point(-100, 100); + const double depth(10); + camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal); + Matrix expectedDpose = numericalDerivative41(backproject, pose, *K, point, depth); + Matrix expectedDcal = numericalDerivative42(backproject, pose, *K, point, depth); + Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth); + Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K, point, depth); + + EXPECT(assert_equal(expectedDpose, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDcal, Dcal, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index 28b7ddac6..f32fe60ed 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -28,7 +28,7 @@ using namespace gtsam; /* ************************************************************************* */ #include TEST(PinholeSet, Stereo) { - typedef vector ZZ; + typedef Point2Vector ZZ; PinholeSet set; CalibratedCamera camera; set.push_back(camera); @@ -72,7 +72,7 @@ TEST(PinholeSet, Stereo) { #include TEST(PinholeSet, Pinhole) { typedef PinholeCamera Camera; - typedef vector ZZ; + typedef Point2Vector ZZ; PinholeSet set; Camera camera; set.push_back(camera); diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index e2a5bcdea..8b9e8a7e6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -100,12 +100,12 @@ TEST( Point2, expmap) { /* ************************************************************************* */ TEST( Point2, arithmetic) { - EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); - EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); - EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); - EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); - EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); - EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); + EXPECT(assert_equal(Point2(-5, -6), -Point2(5, 6))); + EXPECT(assert_equal(Point2(5, 6), Point2(4, 5) + Point2(1, 1))); + EXPECT(assert_equal(Point2(3, 4), Point2(4, 5) - Point2(1, 1))); + EXPECT(assert_equal(Point2(8, 6), Point2(4, 3) * 2)); + EXPECT(assert_equal(Point2(4, 6), 2.0 * Point2(2, 3))); + EXPECT(assert_equal(Point2(2, 3), Point2(4, 6) / 2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 10fd431bc..ca550cdc7 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -743,7 +743,7 @@ namespace { /* ************************************************************************* */ struct Triangle { size_t i_,j_,k_;}; - boost::optional align2(const vector& ps, const vector& qs, + boost::optional align2(const Point2Vector& ps, const Point2Vector& qs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; vector correspondences; @@ -755,7 +755,7 @@ namespace { TEST(Pose2, align_4) { using namespace align_3; - vector ps,qs; + Point2Vector ps,qs; ps += p1, p2, p3; qs += q3, q1, q2; // note in 3,1,2 order ! diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 97ccbcb34..98d3e11ee 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -708,6 +708,27 @@ TEST(Pose3, Bearing2) { EXPECT(assert_equal(expectedH2, actualH2)); } +TEST(Pose3, PoseToPoseBearing) { + Matrix expectedH1, actualH1, expectedH2, actualH2, H2block; + EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2); + + // Since the second pose is treated as a point, the value calculated by + // numericalDerivative22 only depends on the position of the pose. Here, we + // calculate the Jacobian w.r.t. the second pose's position, and then augment + // that with zeroes in the block that is w.r.t. the second pose's + // orientation. + H2block = numericalDerivative22(bearing_proxy, xl1, l2); + expectedH2 = Matrix(2, 6); + expectedH2.setZero(); + expectedH2.block<2, 3>(0, 3) = H2block; + + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ TEST( Pose3, unicycle ) { diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 497599a6a..bc432cad3 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject) CHECK(assert_equal(expected2,actual2,1e-7)); } +/* ************************************************************************* */ +TEST( StereoCamera, projectCheirality) +{ + // create a Stereo camera + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam(Pose3(), K); + + // point behind the camera + Point3 p(0, 0, -5); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException); +#else // otherwise project should not throw the exception + StereoPoint2 expected = StereoPoint2(320, 470, 240); + CHECK(assert_equal(expected,stereoCam.project2(p),1e-7)); +#endif +} + /* ************************************************************************* */ TEST( StereoCamera, backproject_case1) { diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f57ca60d3..97412f94d 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -60,7 +60,7 @@ Point2 z2 = camera2.project(landmark); TEST( triangulation, twoPoses) { vector poses; - vector measurements; + Point2Vector measurements; poses += pose1, pose2; measurements += z1, z2; @@ -108,7 +108,7 @@ TEST( triangulation, twoPosesBundler) { Point2 z2 = camera2.project(landmark); vector poses; - vector measurements; + Point2Vector measurements; poses += pose1, pose2; measurements += z1, z2; @@ -132,7 +132,7 @@ TEST( triangulation, twoPosesBundler) { //****************************************************************************** TEST( triangulation, fourPoses) { vector poses; - vector measurements; + Point2Vector measurements; poses += pose1, pose2; measurements += z1, z2; @@ -195,8 +195,8 @@ TEST( triangulation, fourPoses_distinct_Ks) { Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - vector cameras; - vector measurements; + CameraSet cameras; + Point2Vector measurements; cameras += camera1, camera2; measurements += z1, z2; @@ -260,8 +260,8 @@ TEST( triangulation, outliersAndFarLandmarks) { Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - vector cameras; - vector measurements; + CameraSet cameras; + Point2Vector measurements; cameras += camera1, camera2; measurements += z1, z2; @@ -308,7 +308,7 @@ TEST( triangulation, twoIdenticalPoses) { Point2 z1 = camera1.project(landmark); vector poses; - vector measurements; + Point2Vector measurements; poses += pose1, pose1; measurements += z1, z1; @@ -323,7 +323,7 @@ TEST( triangulation, onePose) { // because there's only one camera observation vector poses; - vector measurements; + Point2Vector measurements; poses += Pose3(); measurements += Point2(0,0); @@ -354,7 +354,7 @@ TEST( triangulation, StereotriangulateNonlinear ) { cameras.push_back(StereoCamera(Pose3(m1), stereoK)); cameras.push_back(StereoCamera(Pose3(m2), stereoK)); - vector measurements; + StereoPoint2Vector measurements; measurements += StereoPoint2(226.936, 175.212, 424.469); measurements += StereoPoint2(339.571, 285.547, 669.973); @@ -378,12 +378,12 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK))); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK))); + graph.emplace_shared(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); + graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); - graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); + graph.emplace_shared >(Symbol('x',2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); @@ -399,8 +399,8 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(TriangulationFactor(cameras[0], measurements[0], unit, Symbol('l',1))); - graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + graph.emplace_shared >(cameras[0], measurements[0], unit, Symbol('l',1)); + graph.emplace_shared >(cameras[1], measurements[1], unit, Symbol('l',1)); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 912b26262..542aca038 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -314,15 +314,24 @@ TEST(Unit3, basis) { Unit3 p(0.1, -0.2, 0.9); Matrix expected(3, 2); - expected << 0.0, -0.994169047, 0.97618706, - -0.0233922129, 0.216930458, 0.105264958; + expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958; Matrix62 actualH; - Matrix actual = p.basis(actualH); - EXPECT(assert_equal(expected, actual, 1e-6)); - Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + boost::bind(BasisTest, _1, boost::none), p); + + // without H, first time + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // without H, cached + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // with H, first time + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + + // with H, cached + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } @@ -426,13 +435,14 @@ TEST(Unit3, ErrorBetweenFactor) { // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); for (size_t i = 0; i < data.size(); i++) { - graph.add(PriorFactor(U(i), data[i], R_prior)); + graph.emplace_shared >(U(i), data[i], R_prior); } // Add process factors using the dot product error function. SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); for (size_t i = 0; i < data.size() - 1; i++) { - Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + Expression exp(Expression(U(i)), &Unit3::errorVector, + Expression(U(i + 1))); graph.addExpressionFactor(R_process, Vector2::Zero(), exp); } diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a1dc3269a..a5d2e04cd 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -24,8 +24,8 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol) { + const std::vector>& projection_matrices, + const Point2Vector& measurements, double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -53,8 +53,8 @@ Vector4 triangulateHomogeneousDLT( return v; } -Point3 triangulateDLT(const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol) { +Point3 triangulateDLT(const std::vector>& projection_matrices, + const Point2Vector& measurements, double rank_tol) { Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 81c8ed2f3..535572fe1 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include @@ -52,8 +53,8 @@ public: * @return Triangulated point, in homogeneous coordinates */ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( - const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol = 1e-9); + const std::vector>& projection_matrices, + const Point2Vector& measurements, double rank_tol = 1e-9); /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 @@ -63,8 +64,9 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol = 1e-9); + const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol = 1e-9); /** * Create a factor graph with projection factors from poses and one calibration @@ -78,7 +80,7 @@ GTSAM_EXPORT Point3 triangulateDLT( template std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, - const std::vector& measurements, Key landmarkKey, + const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value @@ -89,8 +91,8 @@ std::pair triangulationGraph( const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } return std::make_pair(graph, values); } @@ -106,8 +108,8 @@ std::pair triangulationGraph( */ template std::pair triangulationGraph( - const std::vector& cameras, - const std::vector& measurements, Key landmarkKey, + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value @@ -116,8 +118,8 @@ std::pair triangulationGraph( traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } return std::make_pair(graph, values); } @@ -125,8 +127,8 @@ std::pair triangulationGraph( /// PinholeCamera specific version // TODO: (chris) why does this exist? template std::pair triangulationGraph( - const std::vector >& cameras, - const std::vector& measurements, Key landmarkKey, + const CameraSet >& cameras, + const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate) { return triangulationGraph > // (cameras, measurements, landmarkKey, initialEstimate); @@ -153,7 +155,7 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, template Point3 triangulateNonlinear(const std::vector& poses, boost::shared_ptr sharedCal, - const std::vector& measurements, const Point3& initialEstimate) { + const Point2Vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values Values values; @@ -173,8 +175,8 @@ Point3 triangulateNonlinear(const std::vector& poses, */ template Point3 triangulateNonlinear( - const std::vector& cameras, - const std::vector& measurements, const Point3& initialEstimate) { + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values Values values; @@ -188,8 +190,8 @@ Point3 triangulateNonlinear( /// PinholeCamera specific version // TODO: (chris) why does this exist? template Point3 triangulateNonlinear( - const std::vector >& cameras, - const std::vector& measurements, const Point3& initialEstimate) { + const CameraSet >& cameras, + const Point2Vector& measurements, const Point3& initialEstimate) { return triangulateNonlinear > // (cameras, measurements, initialEstimate); } @@ -211,6 +213,8 @@ struct CameraProjectionMatrix { } private: const Matrix3 K_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -228,7 +232,7 @@ private: template Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedCal, - const std::vector& measurements, double rank_tol = 1e-9, + const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false) { assert(poses.size() == measurements.size()); @@ -236,7 +240,7 @@ Point3 triangulatePoint3(const std::vector& poses, throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - std::vector projection_matrices; + std::vector> projection_matrices; CameraProjectionMatrix createP(*sharedCal); // partially apply for(const Pose3& pose: poses) projection_matrices.push_back(createP(pose)); @@ -275,8 +279,8 @@ Point3 triangulatePoint3(const std::vector& poses, */ template Point3 triangulatePoint3( - const std::vector& cameras, - const std::vector& measurements, double rank_tol = 1e-9, + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false) { size_t m = cameras.size(); @@ -286,7 +290,7 @@ Point3 triangulatePoint3( throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - std::vector projection_matrices; + std::vector> projection_matrices; for(const CAMERA& camera: cameras) projection_matrices.push_back( CameraProjectionMatrix(camera.calibration())( @@ -312,8 +316,8 @@ Point3 triangulatePoint3( /// Pinhole-specific version template Point3 triangulatePoint3( - const std::vector >& cameras, - const std::vector& measurements, double rank_tol = 1e-9, + const CameraSet >& cameras, + const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false) { return triangulatePoint3 > // (cameras, measurements, rank_tol, optimize); @@ -453,8 +457,8 @@ private: /// triangulateSafe: extensive checking of the outcome template -TriangulationResult triangulateSafe(const std::vector& cameras, - const std::vector& measured, +TriangulationResult triangulateSafe(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measured, const TriangulationParameters& params) { size_t m = cameras.size(); diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index e04af1339..96d191419 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -83,26 +83,28 @@ namespace gtsam { std::string parent = out.str(); parent += "[label=\""; - for(Key index: clique->conditional_->frontals()) { - if(!first) parent += ","; first = false; + for (Key index : clique->conditional_->frontals()) { + if (!first) parent += ","; + first = false; parent += indexFormatter(index); } - if(clique->parent()){ + if (clique->parent()) { parent += " : "; s << parentnum << "->" << num << "\n"; } first = true; - for(Key sep: clique->conditional_->parents()) { - if(!first) parent += ","; first = false; + for (Key sep : clique->conditional_->parents()) { + if (!first) parent += ","; + first = false; parent += indexFormatter(sep); } parent += "\"];\n"; s << parent; parentnum = num; - for(sharedClique c: clique->children) { + for (sharedClique c : clique->children) { num++; saveGraph(s, c, indexFormatter, parentnum); } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index eae886785..055a03939 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -17,9 +17,13 @@ #pragma once -#include +#include +#include #include #include +#include + +#include namespace gtsam { diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 38f30f5a3..016488701 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,16 +22,17 @@ #pragma once +#include +#include +#include + #include #include #include #include -#include -#include -#include -#include -#include +#include +#include namespace gtsam { @@ -151,7 +152,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(boost::shared_ptr factor) { factors_.push_back(boost::shared_ptr(factor)); } @@ -159,15 +160,22 @@ namespace gtsam { void push_back(const sharedFactor& factor) { factors_.push_back(factor); } + /** Emplace a factor */ + template + typename std::enable_if::value>::type + emplace_shared(Args&&... args) { + factors_.push_back(boost::make_shared(std::forward(args)...)); + } + /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } /** push back many factors as shared_ptr's in a container (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } @@ -175,22 +183,24 @@ namespace gtsam { /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived * classes in favor of a type-specialized version that calls this templated function. */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const BayesTree& bayesTree) { bayesTree.addFactorsToGraph(*this); } +//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid * the copy). */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } +//#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f); @@ -198,14 +208,14 @@ namespace gtsam { /** push back many factors as non-pointer objects in a container (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if, + typename std::enable_if::value, boost::assign::list_inserter > >::type operator+=(boost::shared_ptr factor) { return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); @@ -226,7 +236,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type add(boost::shared_ptr factor) { push_back(factor); } @@ -355,4 +365,4 @@ namespace gtsam { } // namespace gtsam -#include \ No newline at end of file +#include diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index bf2cacc84..4471567fc 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -22,43 +22,48 @@ namespace gtsam { - /* ************************************************************************* */ - template - void ISAM::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function) - { - // Remove the contaminated part of the Bayes tree - BayesNetType bn; - if (!this->empty()) { - const KeySet newFactorKeys = newFactors.keys(); - this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); - } - - // Add the removed top and the new factors - FactorGraphType factors; - factors += bn; - factors += newFactors; - - // Add the orphaned subtrees - for(const sharedClique& orphan: orphans) - factors += boost::make_shared >(orphan); - - // eliminate into a Bayes net - const VariableIndex varIndex(factors); - const KeySet newFactorKeys = newFactors.keys(); - const Ordering constrainedOrdering = - Ordering::ColamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); - Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); - this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); - this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); +/* ************************************************************************* */ +template +void ISAM::update_internal(const FactorGraphType& newFactors, + Cliques& orphans, const Eliminate& function) { + // Remove the contaminated part of the Bayes tree + BayesNetType bn; + const KeySet newFactorKeys = newFactors.keys(); + if (!this->empty()) { + std::vector keyVector(newFactorKeys.begin(), newFactorKeys.end()); + this->removeTop(keyVector, bn, orphans); } - /* ************************************************************************* */ - template - void ISAM::update(const FactorGraphType& newFactors, const Eliminate& function) - { - Cliques orphans; - this->update_internal(newFactors, orphans, function); - } + // Add the removed top and the new factors + FactorGraphType factors; + factors += bn; + factors += newFactors; + + // Add the orphaned subtrees + for (const sharedClique& orphan : orphans) + factors += boost::make_shared >(orphan); + + // Get an ordering where the new keys are eliminated last + const VariableIndex index(factors); + const Ordering ordering = Ordering::ColamdConstrainedLast(index, + std::vector(newFactorKeys.begin(), newFactorKeys.end())); + + // eliminate all factors (top, added, orphans) into a new Bayes tree + auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); + + // Re-add into Bayes tree data structures + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); +} + +/* ************************************************************************* */ +template +void ISAM::update(const FactorGraphType& newFactors, + const Eliminate& function) { + Cliques orphans; + this->update_internal(newFactors, orphans, function); +} } /// namespace gtsam diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index e12c32df3..b94f01689 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -53,8 +53,19 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, gttic(Ordering_COLAMDConstrained); gttic(Prepare); + const size_t nVars = variableIndex.size(); + if (nVars == 0) + { + return Ordering(); + } + + if (nVars == 1) + { + return Ordering(std::vector(1, variableIndex.begin()->first)); + } + const size_t nEntries = variableIndex.nEntries(), nFactors = - variableIndex.nFactors(), nVars = variableIndex.size(); + variableIndex.nFactors(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, (int) nVars); /* colamd arg 3: size of the array A */ diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 2d7ede8e7..6b5bfcc03 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -26,10 +26,9 @@ namespace gtsam { /** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. + * Character and index key used to refer to variables. Will simply cast to a Key, + * i.e., a large integer. Keys are used to retrieve values from Values, + * specify what variables factors depend on, etc. */ class GTSAM_EXPORT Symbol { protected: diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index eefb6302f..60187d129 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -12,11 +12,13 @@ /** * @file GaussianConditional.cpp * @brief Conditional Gaussian Base class - * @author Christian Potthast + * @author Christian Potthast, Frank Dellaert */ -#include -#include +#include +#include +#include + #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -28,9 +30,9 @@ #pragma GCC diagnostic pop #endif -#include -#include -#include +#include +#include +#include using namespace std; @@ -54,38 +56,36 @@ namespace gtsam { BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} /* ************************************************************************* */ - void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const - { + void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " Conditional density "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; } cout << endl; cout << formatMatrixIndented(" R = ", get_R()) << endl; - for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { + for (const_iterator it = beginParents() ; it != endParents() ; ++it) { cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) << endl; } cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; - if(model_) + if (model_) model_->print(" Noise model: "); else cout << " No noise model" << endl; } /* ************************************************************************* */ - bool GaussianConditional::equals(const GaussianFactor& f, double tol) const - { - if (const GaussianConditional* c = dynamic_cast(&f)) - { + bool GaussianConditional::equals(const GaussianFactor& f, double tol) const { + if (const GaussianConditional* c = dynamic_cast(&f)) { // check if the size of the parents_ map is the same if (parents().size() != c->parents().size()) return false; // check if R_ and d_ are linear independent for (DenseIndex i = 0; i < Ab_.rows(); i++) { - list rows1; rows1.push_back(Vector(get_R().row(i))); - list rows2; rows2.push_back(Vector(c->get_R().row(i))); + list rows1, rows2; + rows1.push_back(Vector(get_R().row(i))); + rows2.push_back(Vector(c->get_R().row(i))); // check if the matrices are the same // iterate over the parents_ map @@ -109,16 +109,13 @@ namespace gtsam { return false; return true; - } - else - { + } else { return false; } } /* ************************************************************************* */ - VectorValues GaussianConditional::solve(const VectorValues& x) const - { + VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables const Vector xS = x.vector(FastVector(beginParents(), endParents())); @@ -146,8 +143,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solveOtherRHS( - const VectorValues& parents, const VectorValues& rhs) const - { + const VectorValues& parents, const VectorValues& rhs) const { // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); @@ -159,13 +155,13 @@ namespace gtsam { Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas - if(model_) + if (model_) soln.array() *= model_->sigmas().array(); // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -174,8 +170,7 @@ namespace gtsam { } /* ************************************************************************* */ - void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const - { + void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(FastVector(beginFrontals(), endFrontals())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); @@ -186,25 +181,24 @@ namespace gtsam { gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas - if(model_) + if (model_) frontalVec.array() *= model_->sigmas().array(); // Write frontal solution into a VectorValues DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal)); vectorPosition += getDim(frontal); } } /* ************************************************************************* */ - void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const - { + void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); vectorPosition += getDim(frontal); } } -} +} // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 06c8f3f64..56a5dc085 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -188,7 +188,7 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( m += factor->rows(); } -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) for(DenseIndex d: varDims) { assert(d != numeric_limits::max()); } @@ -219,15 +219,13 @@ FastVector _convertOrCastToJacobians( /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, boost::optional ordering, - boost::optional variableSlots) { + boost::optional p_variableSlots) { gttic(JacobianFactor_combine_constructor); // Compute VariableSlots if one was not provided - boost::optional computedVariableSlots; - if (!variableSlots) { - computedVariableSlots = VariableSlots(graph); - variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots - } + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = + p_variableSlots ? p_variableSlots.get() : VariableSlots(graph); // Cast or convert to Jacobians FastVector jacobians = _convertOrCastToJacobians( @@ -238,15 +236,15 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then // be added after all of the ordered variables. FastVector orderedSlots; - orderedSlots.reserve(variableSlots->size()); + orderedSlots.reserve(variableSlots.size()); if (ordering) { // If an ordering is provided, arrange the slots first that ordering FastList unorderedSlots; size_t nOrderingSlotsUsed = 0; orderedSlots.resize(ordering->size()); FastMap inverseOrdering = ordering->invert(); - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) { + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) { FastMap::const_iterator orderingPosition = inverseOrdering.find(item->first); if (orderingPosition == inverseOrdering.end()) { @@ -267,8 +265,8 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, } else { // If no ordering is provided, arrange the slots as they were, which will be sorted // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) orderedSlots.push_back(item); } gttoc(Order_slots); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 14ff46241..d7814f541 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -154,7 +154,7 @@ namespace gtsam { explicit JacobianFactor( const GaussianFactorGraph& graph, boost::optional ordering = boost::none, - boost::optional variableSlots = boost::none); + boost::optional p_variableSlots = boost::none); /** Virtual destructor */ virtual ~JacobianFactor() {} diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index db5c25442..5312da34b 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -70,9 +70,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, iterator first = begin(); if (ordering) first += ordering->size(); if (first != end()) std::sort(first, end()); - - // Filter out keys with zero dimensions (if ordering had more keys) - erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 35afddb3a..b037aad92 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -160,28 +160,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector VectorValues::vector(const FastVector& keys) const - { - // Count dimensions and collect pointers to avoid double lookups - DenseIndex totalDim = 0; - FastVector items(keys.size()); - for(size_t i = 0; i < keys.size(); ++i) { - items[i] = &at(keys[i]); - totalDim += items[i]->size(); - } - - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - for(const Vector *v: items) { - result.segment(pos, v->size()) = *v; - pos += v->size(); - } - - return result; - } - /* ************************************************************************* */ Vector VectorValues::vector(const Dims& keys) const { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index cb1e08f2d..f187b56de 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -26,6 +26,9 @@ #include +#include +#include + namespace gtsam { /** @@ -43,10 +46,6 @@ namespace gtsam { * - \ref exists (Key) to check if a variable is present * - Other facilities like iterators, size(), dim(), etc. * - * Indices can be non-consecutive and inserted out-of-order, but you should not - * use indices that are larger than a reasonable array size because the indices - * correspond to positions in an internal array. - * * Example: * \code VectorValues values; @@ -64,12 +63,6 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * Internally, all vector values are stored as part of one large vector. In - * gtsam this vector is always pre-allocated for efficiency, using the - * advanced interface described below. Accessing and modifying already-allocated - * values is \f$ O(1) \f$. Using the insert() function of the standard interface - * is slow because it requires re-allocating the internal vector. - * * For advanced usage, or where speed is important: * - Allocate space ahead of time using a pre-allocating constructor * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), @@ -88,20 +81,18 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT VectorValues { - protected: + protected: typedef VectorValues This; - typedef ConcurrentMap Values; ///< Typedef for the collection of Vectors making up a VectorValues - Values values_; ///< Collection of Vectors making up this VectorValues + typedef ConcurrentMap Values; ///< Collection of Vectors making up a VectorValues + Values values_; ///< Vectors making up this VectorValues - public: - typedef Values::iterator iterator; ///< Iterator over vector values - typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair - typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair - typedef std::map Dims; + public: + typedef Values::iterator iterator; ///< Iterator over vector values + typedef Values::const_iterator const_iterator; ///< Const iterator over vector values + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Values::value_type value_type; ///< Typedef to pair + typedef value_type KeyValuePair; ///< Typedef to pair + typedef std::map Dims; ///< Keyed vector dimensions /// @name Standard Constructors /// @{ @@ -111,7 +102,8 @@ namespace gtsam { */ VectorValues() {} - /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ + /** Merge two VectorValues into one, this is more efficient than inserting + * elements one by one. */ VectorValues(const VectorValues& first, const VectorValues& second); /** Create from another container holding pair. */ @@ -147,20 +139,26 @@ namespace gtsam { /** Check whether a variable with key \c j exists. */ bool exists(Key j) const { return find(j) != end(); } - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + /** + * Read/write access to the vector value with key \c j, throws + * std::out_of_range if \c j does not exist, identical to operator[](Key). + */ Vector& at(Key j) { iterator item = find(j); - if(item == end()) + if (item == end()) throw std::out_of_range( "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); else return item->second; } - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + /** + * Access the vector value with key \c j (const version), throws + * std::out_of_range if \c j does not exist, identical to operator[](Key). + */ const Vector& at(Key j) const { const_iterator item = find(j); - if(item == end()) + if (item == end()) throw std::out_of_range( "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); else @@ -207,26 +205,30 @@ namespace gtsam { /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { - if(values_.unsafe_erase(var) == 0) - throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + if (values_.unsafe_erase(var) == 0) + throw std::invalid_argument("Requested variable '" + + DefaultKeyFormatter(var) + + "', is not in this VectorValues."); } /** Set all values to zero vectors. */ void setZero(); - iterator begin() { return values_.begin(); } ///< Iterator over variables - const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables - const_iterator end() const { return values_.end(); } ///< Iterator over variables - //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + iterator begin() { return values_.begin(); } ///< Iterator over variables + const_iterator begin() const { return values_.begin(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables + const_iterator end() const { return values_.end(); } ///< Iterator over variables - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + /** + * Return the iterator corresponding to the requested key, or end() if no + * variable is present with this key. + */ iterator find(Key j) { return values_.find(j); } - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + /** + * Return the iterator corresponding to the requested key, or end() if no + * variable is present with this key. + */ const_iterator find(Key j) const { return values_.find(j); } /** print required by Testable for unit testing */ @@ -244,7 +246,26 @@ namespace gtsam { Vector vector() const; /** Access a vector that is a subset of relevant keys. */ - Vector vector(const FastVector& keys) const; + template + Vector vector(const CONTAINER& keys) const { + DenseIndex totalDim = 0; + FastVector items; + items.reserve(keys.end() - keys.begin()); + for (Key key : keys) { + const Vector* v = &at(key); + totalDim += v->size(); + items.push_back(v); + } + + Vector result(totalDim); + DenseIndex pos = 0; + for (const Vector* v : items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; + } /** Access a vector that is a subset of relevant keys, dims version. */ Vector vector(const Dims& dims) const; @@ -319,54 +340,6 @@ namespace gtsam { /// @} - /** - * scale a vector by a scalar - */ - //friend VectorValues operator*(const double a, const VectorValues &v) { - // VectorValues result = VectorValues::SameStructure(v); - // for(Key j = 0; j < v.size(); ++j) - // result.values_[j] = a * v.values_[j]; - // return result; - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // y.values_[j] += alpha * x.values_[j]; - // else - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - //} - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void sqrt(VectorValues &x) { - // for(Key j = 0; j < x.size(); ++j) - // x.values_[j] = x.values_[j].cwiseSqrt(); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { - // if(numerator.size() != denominator.size() || numerator.size() != result.size()) - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < numerator.size(); ++j) - // if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) - // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); - // else - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void edivInPlace(VectorValues& x, const VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // x.values_[j].array() /= y.values_[j].array(); - // else - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - //} - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index c2a88bd44..8ed695622 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -38,13 +38,13 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation Vector3 biasHat_; ///< Angular rate bias values used during preintegration. Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - /// Default constructor, only for serialization - PreintegratedAhrsMeasurements() {} - friend class AHRSFactor; public: + /// Default constructor, only for serialization and Cython wrapper + PreintegratedAhrsMeasurements() {} + /** * Default constructor, initialize with no measurements * @param bias Current estimate of acceleration and rotation rate biases diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index bcad9d8f7..6fd98bfcb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -83,12 +83,12 @@ public: // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, g)); + return boost::shared_ptr(new Params(Vector3(0, 0, g))); } // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, -g)); + return boost::shared_ptr(new Params(Vector3(0, 0, -g))); } private: @@ -104,6 +104,9 @@ public: ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; protected: @@ -115,7 +118,6 @@ public: */ Eigen::Matrix preintMeasCov_; - PreintegratedCombinedMeasurements() {} friend class CombinedImuFactor; @@ -123,6 +125,9 @@ public: /// @name Constructors /// @{ + /// Default constructor only for serialization and Cython wrapper + PreintegratedCombinedMeasurements() {} + /** * Default constructor, initializes the class with no measurements * @param bias Current estimate of acceleration and rotation rate biases @@ -193,6 +198,9 @@ public: ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 2ad3c17dd..5fc82f0a3 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -169,6 +169,9 @@ private: ar & BOOST_SERIALIZATION_NVP(biasGyro_); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @} }; // ConstantBias class diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 532abdac0..ccf5db5c3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -78,13 +78,13 @@ protected: Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). - /// Default constructor for serialization +public: + + /// Default constructor for serialization and Cython wrapper PreintegratedImuMeasurements() { preintMeasCov_.setZero(); } -public: - /** * Constructor, initializes the class with no measurements * @param bias Current estimate of acceleration and rotation rate biases @@ -106,6 +106,10 @@ public: preintMeasCov_(preintMeasCov) { } + /// Virtual destructor + virtual ~PreintegratedImuMeasurements() { + } + /// print void print(const std::string& s = "Preintegrated Measurements:") const override; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 88d9c6437..025278c81 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,6 +35,8 @@ struct PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + virtual ~PreintegratedRotationParams() {} + virtual void print(const std::string& s) const; virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; @@ -56,6 +58,9 @@ struct PreintegratedRotationParams { ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -163,6 +168,9 @@ class PreintegratedRotation { ar& BOOST_SERIALIZATION_NVP(deltaRij_); ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; template <> diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index f2b2c0fef..1c673bff5 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -39,12 +39,12 @@ struct PreintegrationParams: PreintegratedRotationParams { // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, g)); + return boost::shared_ptr(new PreintegrationParams(Vector3(0, 0, g))); } // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, -g)); + return boost::shared_ptr(new PreintegrationParams(Vector3(0, 0, -g))); } void print(const std::string& s) const; @@ -74,6 +74,9 @@ protected: ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(n_gravity); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace gtsam diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index e4b380ff9..acb8f46f5 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,10 +57,13 @@ class Scenario { class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] - ConstantTwistScenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} + ConstantTwistScenario(const Vector3& w, const Vector3& v, + const Pose3& nTb0 = Pose3()) + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {} - Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } + Pose3 pose(double t) const override { + return nTb0_ * Pose3::Expmap(twist_ * t); + } Vector3 omega_b(double t) const override { return twist_.head<3>(); } Vector3 velocity_n(double t) const override { return rotation(t).matrix() * twist_.tail<3>(); @@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario { private: const Vector6 twist_; const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b + const Pose3 nTb0_; }; /// Accelerating from an arbitrary initial state, with optional rotation diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index dddafad7a..da49e4ddd 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -42,7 +42,7 @@ class TangentPreintegration : public PreintegrationBase { } public: - /// @name Constructors + /// @name Constructors/destructors /// @{ /** @@ -53,6 +53,10 @@ public: TangentPreintegration(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + /// Virtual destructor + virtual ~TangentPreintegration() { + } + /// @} /// @name Basic utilities @@ -135,6 +139,9 @@ private: 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())); } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index d1dc6316d..6f2d60f2f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -781,8 +781,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factors - graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); - graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + graph.emplace_shared(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, biasNoiseModel); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index bc965b058..161b8841a 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include #include @@ -96,9 +96,33 @@ TEST(Scenario, Loop) { const double R = v / w; const Pose3 T30 = scenario.pose(30); EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz())); EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, LoopWithInitialPose) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + const Vector3 W(0, -w, 0), V(v, 0, 0); + const Rot3 nRb0 = Rot3::yaw(M_PI); + const Pose3 nTb0(nRb0, Point3(1, 2, 3)); + const ConstantTwistScenario scenario(W, V, nTb0); + + const double T = 30; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = scenario.pose(30); + EXPECT( + assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9)); +} + /* ************************************************************************* */ TEST(Scenario, Accelerating) { // Set up body pointing towards y axis, and start at 10,20,0 with velocity diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 259bb1efe..22172e44f 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -134,8 +134,10 @@ T Expression::value(const Values& values, if (H) { // Call private version that returns derivatives in H - KeysAndDims pair = keysAndDims(); - return valueAndDerivatives(values, pair.first, pair.second, *H); + KeyVector keys; + FastVector dims; + boost::tie(keys, dims) = keysAndDims(); + return valueAndDerivatives(values, keys, dims, *H); } else // no derivatives needed, just return value return root_->value(values); diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04836a1cb..64a8a6bb6 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -47,7 +47,13 @@ protected: public: typedef boost::shared_ptr > shared_ptr; - /// Constructor + /** + * Constructor: creates a factor from a measurement and measurement function + * @param noiseModel the noise model associated with a measurement + * @param measurement actual value of the measurement, of type T + * @param expression predicts the measurement from Values + * The keys associated with the factor, returned by keys(), are sorted. + */ ExpressionFactor(const SharedNoiseModel& noiseModel, // const T& measurement, const Expression& expression) : NoiseModelFactor(noiseModel), measured_(measurement) { @@ -158,7 +164,18 @@ protected: // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now - boost::tie(keys_, dims_) = expression_.keysAndDims(); + if (keys_.empty()) { + // This is the case when called in ExpressionFactor Constructor. + // We then take the keys from the expression in sorted order. + boost::tie(keys_, dims_) = expression_.keysAndDims(); + } else { + // This happens with classes derived from BinaryExpressionFactor etc. + // In that case, the keys_ are already defined and we just need to grab + // the dimensions in the correct order. + std::map keyedDims; + expression_.dims(keyedDims); + for (Key key : keys_) dims_.push_back(keyedDims[key]); + } } /// Recreate expression from keys_ and measured_, used in load below. @@ -196,9 +213,9 @@ template struct traits > : public Testable > {}; /** - * Binary specialization of ExpressionFactor meant as a base class for binary factors - * Enforces expression method with two keys, and provides evaluateError - * Derived needs to call initialize. + * Binary specialization of ExpressionFactor meant as a base class for binary + * factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'. + * Derived class (a binary factor!) needs to call 'initialize'. */ template class ExpressionFactor2 : public ExpressionFactor { @@ -248,4 +265,3 @@ class ExpressionFactor2 : public ExpressionFactor { // ExpressionFactor2 }// \ namespace gtsam - diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index a0fc117d9..8a8813af5 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -11,120 +11,89 @@ /** * @file ISAM2-impl.cpp - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess * @author Richard Roberts */ -#include -#include // for selective linearization thresholds #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB +#include // for selective linearization thresholds +#include -#include #include +#include +#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter) -{ - const bool debug = ISDEBUG("ISAM2 AddVariables"); +void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, + bool useUnusedSlots, + NonlinearFactorGraph* nonlinearFactors, + FactorIndices* newFactorIndices) { + newFactorIndices->resize(newFactors.size()); - theta.insert(newTheta); - if(debug) newTheta.print("The new variables are: "); - // Add zeros into the VectorValues - delta.insert(newTheta.zeroVectors()); - deltaNewton.insert(newTheta.zeroVectors()); - RgProd.insert(newTheta.zeroVectors()); -} - -/* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) -{ - newFactorIndices.resize(newFactors.size()); - - if(useUnusedSlots) - { + if (useUnusedSlots) { size_t globalFactorIndex = 0; - for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) - { + for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); + ++newFactorIndex) { // Loop to find the next available factor slot - do - { - // If we need to add more factors than we have room for, resize nonlinearFactors, - // filling the new slots with NULL factors. Otherwise, check if the current - // factor in nonlinearFactors is already used, and if so, increase - // globalFactorIndex. If the current factor in nonlinearFactors is unused, break - // out of the loop and use the current slot. - if(globalFactorIndex >= nonlinearFactors.size()) - nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex); - else if(nonlinearFactors[globalFactorIndex]) - ++ globalFactorIndex; + do { + // If we need to add more factors than we have room for, resize + // nonlinearFactors, filling the new slots with NULL factors. Otherwise, + // check if the current factor in nonlinearFactors is already used, and + // if so, increase globalFactorIndex. If the current factor in + // nonlinearFactors is unused, break out of the loop and use the current + // slot. + if (globalFactorIndex >= nonlinearFactors->size()) + nonlinearFactors->resize(nonlinearFactors->size() + + newFactors.size() - newFactorIndex); + else if ((*nonlinearFactors)[globalFactorIndex]) + ++globalFactorIndex; else break; - } while(true); + } while (true); // Use the current slot, updating nonlinearFactors and newFactorSlots. - nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; - newFactorIndices[newFactorIndex] = globalFactorIndex; + (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; + (*newFactorIndices)[newFactorIndex] = globalFactorIndex; } - } - else - { + } else { // We're not looking for unused slots, so just add the factors at the end. - for(size_t i = 0; i < newFactors.size(); ++i) - newFactorIndices[i] = i + nonlinearFactors.size(); - nonlinearFactors.push_back(newFactors); + for (size_t i = 0; i < newFactors.size(); ++i) + (*newFactorIndices)[i] = i + nonlinearFactors->size(); + nonlinearFactors->push_back(newFactors); } } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, - Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables) -{ - variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); - for(Key key: unusedKeys) { - delta.erase(key); - deltaNewton.erase(key); - RgProd.erase(key); - replacedKeys.erase(key); - nodes.unsafe_erase(key); - theta.erase(key); - fixedVariables.erase(key); - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) -{ +KeySet ISAM2::Impl::CheckRelinearizationFull( + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - if(const double* threshold = boost::get(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { + if (const double* threshold = boost::get(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { double maxDelta = key_delta.second.lpNorm(); - if(maxDelta >= *threshold) - relinKeys.insert(key_delta.first); + if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); } - } - else if(const FastMap* thresholds = boost::get >(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { - const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; - if(threshold.rows() != key_delta.second.rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); - if((key_delta.second.array().abs() > threshold.array()).any()) + } else if (const FastMap* thresholds = + boost::get >(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + const Vector& threshold = + thresholds->find(Symbol(key_delta.first).chr())->second; + if (threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(key_delta.first).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + if ((key_delta.second.array().abs() > threshold.array()).any()) relinKeys.insert(key_delta.first); } } @@ -133,167 +102,117 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, - const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) -{ +static void CheckRelinearizationRecursiveDouble( + double threshold, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { // Check the current clique for relinearization bool relinearize = false; - for(Key var: *clique->conditional()) { + for (Key var : *clique->conditional()) { double maxDelta = delta[var].lpNorm(); - if(maxDelta >= threshold) { - relinKeys.insert(var); + if (maxDelta >= threshold) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); } } } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, - const VectorValues& delta, - const ISAM2Clique::shared_ptr& clique) -{ +static void CheckRelinearizationRecursiveMap( + const FastMap& thresholds, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { // Check the current clique for relinearization bool relinearize = false; - for(Key var: *clique->conditional()) { + for (Key var : *clique->conditional()) { // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(var).chr())->second; const Vector& deltaVar = delta[var]; // Verify the threshold vector matches the actual variable size - if(threshold.rows() != deltaVar.rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(var).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); + if (threshold.rows() != deltaVar.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(var).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); // Check for relinearization - if((deltaVar.array().abs() > threshold.array()).any()) { - relinKeys.insert(var); + if ((deltaVar.array().abs() > threshold.array()).any()) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); } } } /* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) -{ +KeySet ISAM2::Impl::CheckRelinearizationPartial( + const ISAM2::Roots& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - for(const ISAM2::sharedClique& root: roots) { - if(relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); - else if(relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, root); + for (const ISAM2::sharedClique& root : roots) { + if (relinearizeThreshold.type() == typeid(double)) + CheckRelinearizationRecursiveDouble( + boost::get(relinearizeThreshold), delta, root, &relinKeys); + else if (relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap( + boost::get >(relinearizeThreshold), delta, root, + &relinKeys); } return relinKeys; } -/* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) -{ - static const bool debug = false; - // does the separator contain any of the variables? - bool found = false; - for(Key key: clique->conditional()->parents()) { - if (markedMask.exists(key)) { - found = true; - break; - } - } - if (found) { - // then add this clique - keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << clique->conditional()->front() << endl; - } - for(const ISAM2Clique::shared_ptr& child: clique->children) { - FindAll(child, keys, markedMask); - } -} - -/* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) -{ - // If debugging, invalidate if requested, otherwise do not invalidate. - // Invalidating means setting expmapped entries to Inf, to trigger assertions - // if we try to re-use them. -#ifdef NDEBUG - invalidateIfDebug = boost::none; -#endif - - assert(values.size() == delta.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for(key_value = values.begin(); key_value != values.end(); ++key_value) - { - key_delta = delta.find(key_value->key); -#else - for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) - { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(delta[var].size() == (int)key_value->value.dim()); - assert(delta[var].allFinite()); - if(mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta[var]); - key_value->value = *retracted; - retracted->deallocate_(); - if(invalidateIfDebug) - (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) - } - } -} - /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { +inline static void optimizeInPlace(const ISAM2::sharedClique& clique, + VectorValues* result) { // parents are assumed to already be solved and available in result - result.update(clique->conditional()->solve(result)); + result->update(clique->conditional()->solve(*result)); // starting from the root, call optimize on each conditional - for(const boost::shared_ptr& child: clique->children) + for (const ISAM2::sharedClique& child : clique->children) optimizeInPlace(child, result); } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { - +size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - for(const ISAM2::sharedClique& root: roots) + for (const ISAM2::sharedClique& root : roots) internal::optimizeInPlace(root, delta); - lastBacksubVariableCount = delta.size(); + lastBacksubVariableCount = delta->size(); } else { // Optimize with wildfire lastBacksubVariableCount = 0; - for(const ISAM2::sharedClique& root: roots) + for (const ISAM2::sharedClique& root : roots) lastBacksubVariableCount += optimizeWildfireNonRecursive( - root, wildfireThreshold, replacedKeys, delta); // modifies delta + root, wildfireThreshold, replacedKeys, delta); // modifies delta -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(size_t j=0; j)).all()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (VectorValues::const_iterator key_delta = delta->begin(); + key_delta != delta->end(); ++key_delta) { + assert((*delta)[key_delta->first].allFinite()); + } #endif } @@ -302,69 +221,77 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, - const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { - +void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, + const VectorValues& grad, VectorValues* RgProd, + size_t* varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - for(Key j: *clique->conditional()) { - if(replacedKeys.exists(j)) { + for (Key j : *clique->conditional()) { + if (replacedKeys.exists(j)) { anyReplaced = true; break; } } - if(anyReplaced) { + if (anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = grad.vector(FastVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); - Vector gS = grad.vector(FastVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); + Vector gR = + grad.vector(FastVector(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals())); + Vector gS = + grad.vector(FastVector(clique->conditional()->beginParents(), + clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; + Vector RSgProd = clique->conditional()->get_R() * gR + + clique->conditional()->get_S() * gS; // Write into RgProd vector DenseIndex vectorPosition = 0; - for(Key frontal: clique->conditional()->frontals()) { - Vector& RgProdValue = RgProd[frontal]; + for (Key frontal : clique->conditional()->frontals()) { + Vector& RgProdValue = (*RgProd)[frontal]; RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); vectorPosition += RgProdValue.size(); } - // Now solve the part of the Newton's method point for this clique (back-substitution) - //(*clique)->solveInPlace(deltaNewton); + // Now solve the part of the Newton's method point for this clique + // (back-substitution) + // (*clique)->solveInPlace(deltaNewton); - varsUpdated += clique->conditional()->nrFrontals(); + *varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - for(const ISAM2Clique::shared_ptr& child: clique->children) { - updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } + for (const ISAM2::sharedClique& child : clique->children) { + updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); + } } } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd) { - +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + const VectorValues& gradAtZero, + VectorValues* RgProd) { // Update variables size_t varsUpdated = 0; - for(const ISAM2::sharedClique& root: roots) { - internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); + for (const ISAM2::sharedClique& root : roots) { + internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, + &varsUpdated); } return varsUpdated; } - + /* ************************************************************************* */ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, - const VectorValues& RgProd) -{ + const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); - + // Compute minimizing step size double RgNormSq = RgProd.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; @@ -373,4 +300,4 @@ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, return step * gradAtZero; } -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 161932344..8a30fb8cd 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -23,7 +23,6 @@ namespace gtsam { struct GTSAM_EXPORT ISAM2::Impl { - struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -32,35 +31,14 @@ struct GTSAM_EXPORT ISAM2::Impl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + boost::optional > constrainedKeys; }; - /** - * Add new variables to the ISAM2 system. - * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param ordering Current ordering to be augmented with new variables - * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the /// complete list of nonlinear factors, and populates the list of new factor indices, both /// optionally finding and reusing empty factor slots. static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices); - - /** - * Remove variables from the ISAM2 system. - */ - static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, - Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables); + NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -85,57 +63,22 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static KeySet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** - * Recursively search this clique and its children for marked keys appearing - * in the separator, and add the *frontal* keys of any cliques whose - * separator contains any marked keys to the set \c keys. The purpose of - * this is to discover the cliques that need to be redone due to information - * propagating to them from cliques that directly contain factors being - * relinearized. - * - * The original comment says this finds all variables directly connected to - * the marked ones by measurements. Is this true, because it seems like this - * would also pull in variables indirectly connected through other frontal or - * separator variables? - * - * Alternatively could we trace up towards the root for each variable here? - */ - static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); - - /** - * Apply expmap to the given values, but only for indices appearing in - * \c markedRelinMask. Values are expmapped in-place. - * \param [in, out] values The value to expmap in-place - * \param delta The linear delta with which to expmap - * \param ordering The ordering - * \param mask Mask on linear indices, only \c true entries are expmapped - * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, - * expmapped deltas will be set to an invalid value (infinity) to catch bugs - * where we might expmap something twice, or expmap it but then not - * recalculate its delta. - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, - boost::optional invalidateIfDebug = boost::none, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /** * Update the Newton's method step point, using wildfire */ - static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); + static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, + const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd); - + const VectorValues& gradAtZero, VectorValues* RgProd); + /** * Compute the gradient-search point. Only used in Dogleg. */ diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h deleted file mode 100644 index f04e16b7d..000000000 --- a/gtsam/nonlinear/ISAM2-inl.h +++ /dev/null @@ -1,311 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ISAM2-inl.h - * @brief - * @author Richard Roberts - * @date Mar 16, 2012 - */ - - -#pragma once - -#include -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Vector& delta = getDelta()[key]; - return traits::Retract(theta_.at(key), delta); -} - -/* ************************************************************************* */ -namespace internal { -template -void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - if(recalculate) { - - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - delta.update(clique->conditional()->solve(delta)); - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - - // Recurse to children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - optimizeWildfire(child, threshold, changed, replaced, delta, count); - } - } -} - -template -bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, potentially refactor - if(recalculate) - { - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - special version stores solution pointers in cliques for fast access. - { - // Create solution part pointers if necessary and possible - necessary if solnPointers_ is - // empty, and possible if either we're a root, or we have a parent with valid solnPointers_. - boost::shared_ptr parent = clique->parent_.lock(); - if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) - { - for(Key key: clique->conditional()->frontals()) - clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); - for(Key key: clique->conditional()->parents()) - clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); - } - - // See if we can use solution part pointers - we can if they either already existed or were - // created above. - if(!clique->solnPointers_.empty()) - { - GaussianConditional& c = *clique->conditional(); - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(clique->conditional()->nrParents()); - for(Key parent: clique->conditional()->parents()) { - parentPointers.push_back(clique->solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for(const VectorValues::const_iterator& parentPointer: parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - clique->solnPointers_.at(*frontal)->second = solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } - else - { - // Just call plain solve because we couldn't use solution pointers. - delta.update(clique->conditional()->solve(delta)); - } - } - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - } - - return recalculate; -} - -} // namespace internal - -/* ************************************************************************* */ -template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - KeySet changed; - int count = 0; - // starting from the root, call optimize on each conditional - if(root) - internal::optimizeWildfire(root, threshold, changed, keys, delta, count); - return count; -} - -/* ************************************************************************* */ -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) -{ - KeySet changed; - size_t count = 0; - - if (root) { - std::stack > travStack; - travStack.push(root); - boost::shared_ptr currentNode = root; - while (!travStack.empty()) { - currentNode = travStack.top(); - travStack.pop(); - bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); - if (recalculate) { - for(const typename CLIQUE::shared_ptr& child: currentNode->children) { - travStack.push(child); - } - } - } - } - - return count; -} - -/* ************************************************************************* */ -template -void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (int)clique->conditional()->rows(); - int dimSep = (int)clique->conditional()->get_S().cols(); - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - nnz_internal(child, result); - } -} - -/* ************************************************************************* */ -template -int calculate_nnz(const boost::shared_ptr& clique) { - int result = 0; - // starting from the root, add up entries of frontal and conditional matrices of each conditional - nnz_internal(clique, result); - return result; -} - -} - diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 18296b393..df07050de 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -10,50 +10,48 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-inl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @file ISAM2.cpp + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ -#include // for operator += -using namespace boost::assign; +#include + +#include +#include +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include + #include #include -#include -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br -#include -#include -#include -#include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include -#include -#include -#include - -#include -#include -#include -#include +#include +#include +#include +#include using namespace std; namespace gtsam { -// Instantiate base classes -template class BayesTreeCliqueBase; +// Instantiate base class template class BayesTree; -static const bool disableReordering = false; -static const double batchThreshold = 0.65; +static const bool kDisableReordering = false; +static const double kBatchThreshold = 0.65; /* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of reeliminating ISAM2 -// subtrees. -class ISAM2BayesTree : public ISAM2::Base -{ -public: +// Special BayesTree class that uses ISAM2 cliques - this is the result of +// reeliminating ISAM2 subtrees. +class ISAM2BayesTree : public ISAM2::Base { + public: typedef ISAM2::Base Base; typedef ISAM2BayesTree This; typedef boost::shared_ptr shared_ptr; @@ -62,129 +60,65 @@ public: }; /* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for reeliminating ISAM2 -// subtrees. -class ISAM2JunctionTree : public JunctionTree -{ -public: +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for +// reeliminating ISAM2 subtrees. +class ISAM2JunctionTree + : public JunctionTree { + public: typedef JunctionTree Base; typedef ISAM2JunctionTree This; typedef boost::shared_ptr shared_ptr; - ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : - Base(eliminationTree) {} + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} }; /* ************************************************************************* */ -std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { - std::string s; - switch (adaptationMode) { - case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: s = "SEARCH_EACH_ITERATION"; break; - case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: s = "ONE_STEP_PER_ITERATION"; break; - default: s = "UNDEFINED"; break; - } - return s; -} - -/* ************************************************************************* */ -DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator(const std::string& adaptationMode) const { - std::string s = adaptationMode; boost::algorithm::to_upper(s); - if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; - if (s == "ONE_STEP_PER_ITERATION") return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; - - /* default is SEARCH_EACH_ITERATION */ - return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; -} - -/* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { - std::string s = str; boost::algorithm::to_upper(s); - if (s == "QR") return ISAM2Params::QR; - if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; - - /* default is CHOLESKY */ - return ISAM2Params::CHOLESKY; -} - -/* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { - std::string s; - switch (value) { - case ISAM2Params::QR: s = "QR"; break; - case ISAM2Params::CHOLESKY: s = "CHOLESKY"; break; - default: s = "UNDEFINED"; break; - } - return s; -} - -/* ************************************************************************* */ -void ISAM2Clique::setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult) -{ - conditional_ = eliminationResult.first; - cachedFactor_ = eliminationResult.second; - // Compute gradient contribution - gradientContribution_.resize(conditional_->cols() - 1); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons - gradientContribution_ << -conditional_->get_R().transpose() * conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); -} - -/* ************************************************************************* */ -bool ISAM2Clique::equals(const This& other, double tol) const { - return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) - || (cachedFactor_ && other.cachedFactor_ - && cachedFactor_->equals(*other.cachedFactor_, tol))); -} - -/* ************************************************************************* */ -void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const -{ - Base::print(s,formatter); - if(cachedFactor_) - cachedFactor_->print(s + "Cached: ", formatter); - else - std::cout << s << "Cached empty" << std::endl; - if(gradientContribution_.rows() != 0) - gtsam::print(gradientContribution_, "Gradient contribution: "); -} - -/* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2Params& params): params_(params), update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ ISAM2::ISAM2() : update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ bool ISAM2::equals(const ISAM2& other, double tol) const { - return Base::equals(other, tol) - && theta_.equals(other.theta_, tol) && variableIndex_.equals(other.variableIndex_, tol) - && nonlinearFactors_.equals(other.nonlinearFactors_, tol) - && fixedVariables_ == other.fixedVariables_; + return Base::equals(other, tol) && theta_.equals(other.theta_, tol) && + variableIndex_.equals(other.variableIndex_, tol) && + nonlinearFactors_.equals(other.nonlinearFactors_, tol) && + fixedVariables_ == other.fixedVariables_; } /* ************************************************************************* */ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; - if(debug) cout << "Getting affected factors for "; - if(debug) { for(const Key key: keys) { cout << key << " "; } } - if(debug) cout << endl; + if (debug) cout << "Getting affected factors for "; + if (debug) { + for (const Key key : keys) { + cout << key << " "; + } + } + if (debug) cout << endl; NonlinearFactorGraph allAffected; KeySet indices; - for(const Key key: keys) { + for (const Key key : keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } - if(debug) cout << "Affected factors are: "; - if(debug) { for(const size_t index: indices) { cout << index << " "; } } - if(debug) cout << endl; + if (debug) cout << "Affected factors are: "; + if (debug) { + for (const size_t index : indices) { + cout << index << " "; + } + } + if (debug) cout << endl; return indices; } @@ -192,9 +126,8 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -GaussianFactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const -{ +GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); @@ -208,29 +141,29 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - for(Key idx: candidates) { + auto linearized = boost::make_shared(); + for (Key idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; - for(Key key: nonlinearFactors_[idx]->keys()) { - if(affectedKeysSet.find(key) == affectedKeysSet.end()) { + for (Key key : nonlinearFactors_[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; } - if(useCachedLinear && relinKeys.find(key) != relinKeys.end()) + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) useCachedLinear = false; } - if(inside) { - if(useCachedLinear) { + if (inside) { + if (useCachedLinear) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_); + auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); #endif @@ -245,12 +178,13 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the affected area +// find intermediate (linearized) factors from cache that are passed into the +// affected area -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { GaussianFactorGraph cachedBoundary; - for(sharedClique orphan: orphans) { + for (sharedClique orphan : orphans) { // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); } @@ -259,27 +193,27 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const vector& observedKeys, - const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result& result) -{ - // TODO: new factors are linearized twice, the newFactors passed in are not used. +boost::shared_ptr ISAM2::recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result* result) { + // TODO(dellaert): new factors are linearized twice, + // the newFactors passed in are not used. const bool debug = ISDEBUG("ISAM2 recalculate"); // Input: BayesTree(this), newFactors -//#define PRINT_STATS // figures for paper, disable for timing +// figures for paper, disable for timing #ifdef PRINT_STATS static int counter = 0; int maxClique = 0; double avgClique = 0; int numCliques = 0; int nnzR = 0; - if (counter>0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); + if (counter > 0) { // cannot call on empty tree + GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); maxClique = cstats.maxCONDITIONALSize; avgClique = cstats.avgCONDITIONALSize; @@ -289,167 +223,200 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke counter++; #endif - if(debug) { + if (debug) { cout << "markedKeys: "; - for(const Key key: markedKeys) { cout << key << " "; } + for (const Key key : markedKeys) { + cout << key << " "; + } cout << endl; cout << "observedKeys: "; - for(const Key key: observedKeys) { cout << key << " "; } + for (const Key key : observedKeys) { + cout << key << " "; + } cout << endl; } // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all parents up to the root. - // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. gttic(removetop); Cliques orphans; GaussianBayesNet affectedBayesNet; - this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); + this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), + affectedBayesNet, orphans); gttoc(removetop); // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached factors get messed up - // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't - // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose - // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected - // in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary + // bug was here: we cannot reuse the original factors, because then the cached + // factors get messed up [all the necessary data is actually contained in the + // affectedBayesNet, including what was passed in from the boundaries, + // so this would be correct; however, in the process we also generate new + // cached_ entries that will be wrong (ie. they don't contain what would be + // passed up at a certain point if batch elimination was done, but that's + // what we need); we could choose not to update cached_ from here, but then + // the new information (and potentially different variable ordering) is not + // reflected in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add the + // cached factors from the boundary // BEGIN OF COPIED CODE - // ordering provides all keys in conditionals, there cannot be others because path to root included + // ordering provides all keys in conditionals, there cannot be others because + // path to root included gttic(affectedKeys); FastList affectedKeys; - for(const ConditionalType::shared_ptr& conditional: affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result + boost::shared_ptr affectedKeysSet( + new KeySet()); // Will return this result - if(affectedKeys.size() >= theta_.size() * batchThreshold) - { + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { // Do a batch step - reorder and relinearize all variables gttic(batch); gttic(add_keys); - br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); + br::copy(variableIndex_ | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); + + // Removed unused keys: + VariableIndex affectedFactorsVarIndex = variableIndex_; + + affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), + unusedIndices.end()); + + for (const Key key : unusedIndices) { + affectedKeysSet->erase(key); + } gttoc(add_keys); gttic(ordering); Ordering order; - if(constrainKeys) - { - order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys); - } - else - { - if(theta_.size() > observedKeys.size()) - { + if (constrainKeys) { + order = + Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); + } else { + if (theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained FastMap constraintGroups; - for(Key var: observedKeys) - constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); - } - else - { - order = Ordering::Colamd(variableIndex_); + for (Key var : observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { + order = Ordering::Colamd(affectedFactorsVarIndex); } } gttoc(ordering); gttic(linearize); GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if(params_.cacheLinearizedFactors) - linearFactors_ = linearized; + if (params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order)) - .eliminate(params_.getEliminationFunction()).first; + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; gttoc(eliminate); gttic(insert); this->clear(); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); - result.variablesReeliminated = affectedKeysSet->size(); - result.factorsRecalculated = nonlinearFactors_.size(); + result->variablesReeliminated = affectedKeysSet->size(); + result->factorsRecalculated = nonlinearFactors_.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); lastAffectedFactorCount = linearized.size(); // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: theta_.keys()) { - result.detail->variableStatus[key].isReeliminated = true; + if (params_.enableDetailedResults) { + for (Key key : theta_.keys()) { + result->detail->variableStatus[key].isReeliminated = true; } } gttoc(batch); } else { - gttic(incremental); // 2. Add the new factors \Factors' into the resulting factor graph FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), + observedKeys.end()); gttic(relinearizeAffected); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); - if(debug) factors.print("Relinearized factors: "); + GaussianFactorGraph factors( + *relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); + if (debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); - if(debug) { cout << "Affected keys: "; for(const Key key: affectedKeys) { cout << key << " "; } cout << endl; } + if (debug) { + cout << "Affected keys: "; + for (const Key key : affectedKeys) { + cout << key << " "; + } + cout << endl; + } // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: affectedAndNewKeys) { - result.detail->variableStatus[key].isReeliminated = true; + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; } } - result.variablesReeliminated = affectedAndNewKeys.size(); - result.factorsRecalculated = factors.size(); + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeys.size(); lastAffectedFactorCount = factors.size(); #ifdef PRINT_STATS // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique - << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; + cout << "linear: #markedKeys: " << markedKeys.size() + << " #affectedVariables: " << affectedKeys.size() + << " #affectedFactors: " << factors.size() + << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique + << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; #endif gttic(cached); // add the cached intermediate results from the boundary of the orphans ... GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if(debug) cachedBoundary.print("Boundary factors: "); + if (debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); gttic(orphans); // Add the orphaned subtrees - for(const sharedClique& orphan: orphans) + for (const sharedClique& orphan : orphans) factors += boost::make_shared >(orphan); gttoc(orphans); - // END OF COPIED CODE - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) gttic(reorder_and_eliminate); gttic(list_to_set); // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in the ordering + // markedKeys are passed in: those variables will be forced to the end in + // the ordering affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); gttoc(list_to_set); @@ -458,37 +425,46 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(ordering_constraints); // Create ordering constraints - FastMap constraintGroups; - if(constrainKeys) { + FastMap constraintGroups; + if (constrainKeys) { constraintGroups = *constrainKeys; } else { - constraintGroups = FastMap(); - const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var: observedKeys) + constraintGroups = FastMap(); + const int group = + observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + for (Key var : observedKeys) constraintGroups.insert(make_pair(var, group)); } // Remove unaffected keys from the constraints - for(FastMap::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); /*Incremented in loop ++iter*/) { - if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter ++); + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (unusedIndices.exists(iter->first) || + !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter++); else - ++ iter; + ++iter; } gttoc(ordering_constraints); // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( - factors, affectedFactorsVarIndex, ordering)).eliminate(params_.getEliminationFunction()).first; + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) + .eliminate(params_.getEliminationFunction()) + .first; gttoc(reorder_and_eliminate); gttic(reassemble); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(reassemble); @@ -498,22 +474,79 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke } // Root clique variables for detailed results - if(params_.enableDetailedResults) { - for(const sharedNode& root: this->roots()) - for(Key var: *root->conditional()) - result.detail->variableStatus[var].inRootClique = true; + if (params_.enableDetailedResults) { + for (const sharedNode& root : this->roots()) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; } return affectedKeysSet; } /* ************************************************************************* */ -ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, - const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, bool force_relinearize) -{ +void ISAM2::addVariables(const Values& newTheta) { + const bool debug = ISDEBUG("ISAM2 AddVariables"); + theta_.insert(newTheta); + if (debug) newTheta.print("The new variables are: "); + // Add zeros into the VectorValues + delta_.insert(newTheta.zeroVectors()); + deltaNewton_.insert(newTheta.zeroVectors()); + RgProd_.insert(newTheta.zeroVectors()); +} + +/* ************************************************************************* */ +void ISAM2::removeVariables(const KeySet& unusedKeys) { + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); + for (Key key : unusedKeys) { + delta_.erase(key); + deltaNewton_.erase(key); + RgProd_.erase(key); + deltaReplacedMask_.erase(key); + Base::nodes_.unsafe_erase(key); + theta_.erase(key); + fixedVariables_.erase(key); + } +} + +/* ************************************************************************* */ +void ISAM2::expmapMasked(const KeySet& mask) { + assert(theta_.size() == delta_.size()); + Values::iterator key_value; + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) { + key_delta = delta_.find(key_value->key); +#else + for (key_value = theta_.begin(), key_delta = delta_.begin(); + key_value != theta_.end(); ++key_value, ++key_delta) { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; + assert(static_cast(delta_[var].size()) == key_value->value.dim()); + assert(delta_[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->value.retract_(delta_[var]); + key_value->value = *retracted; + retracted->deallocate_(); +#ifndef NDEBUG + // If debugging, invalidate delta_ entries to Inf, to trigger assertions + // if we try to re-use them. + delta_[var] = Vector::Constant(delta_[var].rows(), + numeric_limits::infinity()); +#endif + } + } +} + +/* ************************************************************************* */ +ISAM2Result ISAM2::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const FactorIndices& removeFactorIndices, + const boost::optional >& constrainedKeys, + const boost::optional >& noRelinKeys, + const boost::optional >& extraReelimKeys, + bool force_relinearize) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -528,40 +561,44 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; - if(params_.enableDetailedResults) + if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); - const bool relinearizeThisStep = force_relinearize - || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); + const bool relinearizeThisStep = + force_relinearize || (params_.enableRelinearization && + update_count_ % params_.relinearizeSkip == 0); - if(verbose) { + if (verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } // Update delta if we need it to check relinearization later - if(relinearizeThisStep) { + if (relinearizeThisStep) { gttic(updateDelta); - updateDelta(disableReordering); + updateDelta(kDisableReordering); gttoc(updateDelta); } gttic(push_back_factors); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. // Add the new factor indices to the result struct - if(debug || verbose) newFactors.print("The new factors are: "); - Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors_, result.newFactorsIndices); + if (debug || verbose) newFactors.print("The new factors are: "); + Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, + &nonlinearFactors_, &result.newFactorsIndices); // Remove the removed factors - NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for(size_t index: removeFactorIndices) { + NonlinearFactorGraph removeFactors; + removeFactors.reserve(removeFactorIndices.size()); + for (size_t index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(index); + if (params_.cacheLinearizedFactors) linearFactors_.remove(index); } - // Remove removed factors from the variable index so we do not attempt to relinearize them - variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), + removeFactors); // Compute unused keys and indices KeySet unusedKeys; @@ -570,123 +607,144 @@ ISAM2Result ISAM2::update( // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. KeySet removedAndEmpty; - for(Key key: removeFactors.keys()) { - if(variableIndex_[key].empty()) + for (Key key : removeFactors.keys()) { + if (variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } KeySet newFactorSymbKeys = newFactors.keys(); std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(unusedKeys, unusedKeys.end())); // Get indices for unused keys - for(Key key: unusedKeys) { + for (Key key : unusedKeys) { unusedIndices.insert(unusedIndices.end(), key); } } gttoc(push_back_factors); gttic(add_new_variables); - // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); + // 2. Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. + addVariables(newTheta); // New keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } + if (params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + result.detail->variableStatus[key].isNew = true; + } + } gttoc(add_new_variables); gttic(evaluate_error_before); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_before); gttic(gather_involved_keys); // 3. Mark linear update - KeySet markedKeys = newFactors.keys(); // Get keys from new factors + KeySet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors - markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys + KeySet markedRemoveKeys = + removeFactors.keys(); // Get keys involved in removed factors + markedKeys.insert( + markedRemoveKeys.begin(), + markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys - if(extraReelimKeys) { - for(Key key: *extraReelimKeys) { + if (extraReelimKeys) { + for (Key key : *extraReelimKeys) { markedKeys.insert(key); } } // Observed keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: markedKeys) { + if (params_.enableDetailedResults) { + for (Key key : markedKeys) { result.detail->variableStatus[key].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Key value) instead of the iterator constructor. - FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - for(Key index: markedKeys) { - if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused - observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; + observedKeys.reserve(markedKeys.size()); + for (Key index : markedKeys) { + if (unusedIndices.find(index) == + unusedIndices.end()) // Only add if not unused + observedKeys.push_back( + index); // Make a copy of these, as we'll soon add to them } gttoc(gather_involved_keys); - // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + // Check relinearization if we're at the nth step, or we are using a looser + // loop relin threshold KeySet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); - // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if(params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial(roots_, delta_, params_.relinearizeThreshold); + // 4. Mark keys in \Delta above threshold \beta: + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + if (params_.enablePartialRelinearizationCheck) + relinKeys = Impl::CheckRelinearizationPartial( + roots_, delta_, params_.relinearizeThreshold); else - relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging + relinKeys = + Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); + if (kDisableReordering) + relinKeys = Impl::CheckRelinearizationFull( + delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed - for(Key key: fixedVariables_) { + for (Key key : fixedVariables_) { relinKeys.erase(key); } - if(noRelinKeys) { - for(Key key: *noRelinKeys) { + if (noRelinKeys) { + for (Key key : *noRelinKeys) { relinKeys.erase(key); } } // Above relin threshold keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: relinKeys) { + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = true; } } + result.detail->variableStatus[key].isRelinearized = true; + } + } // Add the variables being relinearized to the marked keys KeySet markedRelinMask; - for(const Key key: relinKeys) - markedRelinMask.insert(key); + for (const Key key : relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. if (!relinKeys.empty()) { - for(const sharedClique& root: roots_) + for (const sharedClique& root : roots_) // add other cliques that have the marked ones in the separator - Impl::FindAll(root, markedKeys, markedRelinMask); + root->findAll(markedRelinMask, &markedKeys); // Relin involved keys for detailed results - if(params_.enableDetailedResults) { + if (params_.enableDetailedResults) { KeySet involvedRelinKeys; - for(const sharedClique& root: roots_) - Impl::FindAll(root, involvedRelinKeys, markedRelinMask); - for(Key key: involvedRelinKeys) { - if(!result.detail->variableStatus[key].isAboveRelinThreshold) { + for (const sharedClique& root : roots_) + root->findAll(markedRelinMask, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = true; } } + result.detail->variableStatus[key].isRelinearized = true; + } + } } } gttoc(fluid_find_all); gttic(expmap); - // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, markedRelinMask, delta_); + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (!relinKeys.empty()) expmapMasked(markedRelinMask); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -696,17 +754,15 @@ ISAM2Result ISAM2::update( gttic(linearize_new); // 7. Linearize new factors - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { gttic(linearize); - GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_); - if(params_.findUnusedFactorSlots) - { + auto linearFactors = newFactors.linearize(theta_); + if (params_.findUnusedFactorSlots) { linearFactors_.resize(nonlinearFactors_.size()); - for(size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) - linearFactors_[result.newFactorsIndices[newFactorI]] = (*linearFactors)[newFactorI]; - } - else - { + for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) + linearFactors_[result.newFactorsIndices[newFactorI]] = + (*linearFactors)[newFactorI]; + } else { linearFactors_.push_back(*linearFactors); } assert(nonlinearFactors_.size() == linearFactors_.size()); @@ -716,7 +772,7 @@ ISAM2Result ISAM2::update( gttic(augment_VI); // Augment the variable index with the new factors - if(params_.findUnusedFactorSlots) + if (params_.findUnusedFactorSlots) variableIndex_.augment(newFactors, result.newFactorsIndices); else variableIndex_.augment(newFactors); @@ -724,26 +780,26 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); + boost::shared_ptr replacedKeys; + if (!markedKeys.empty() || !observedKeys.empty()) + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, + unusedIndices, constrainedKeys, &result); // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) + if (replacedKeys) deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); gttoc(recalculate); // Update data structures to remove unused keys - if(!unusedKeys.empty()) { + if (!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, Base::nodes_, fixedVariables_); + removeVariables(unusedKeys); gttoc(remove_variables); } result.cliques = this->nodes().size(); gttic(evaluate_error_after); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_after); @@ -751,36 +807,60 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -void ISAM2::marginalizeLeaves(const FastList& leafKeysList, - boost::optional marginalFactorsIndices, - boost::optional deletedFactorsIndices) -{ +void ISAM2::marginalizeLeaves( + const FastList& leafKeysList, + boost::optional marginalFactorsIndices, + boost::optional deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. -// multimap marginalFactors; + // multimap marginalFactors; map > marginalFactors; - // Keep track of factors that get summarized by removing cliques - KeySet factorIndicesToRemove; - // Keep track of variables removed in subtrees KeySet leafKeysRemoved; + // Keep track of factors that get summarized by removing cliques + KeySet factorIndicesToRemove; + + // Remove the subtree and throw away the cliques + auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { + const Cliques removedCliques = this->removeSubtree(subtreeRoot); + for (const sharedClique& removedClique : removedCliques) { + auto cg = removedClique->conditional(); + marginalFactors.erase(cg->front()); + leafKeysRemoved.insert(cg->beginFrontals(), cg->endFrontals()); + for (Key frontal : cg->frontals()) { + // Add to factors to remove + const auto& involved = variableIndex_[frontal]; + factorIndicesToRemove.insert(involved.begin(), involved.end()); +#if !defined(NDEBUG) + // Check for non-leaf keys + if (!leafKeys.exists(frontal)) + throw std::runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so should " + "no longer be used."); +#endif + } + } + return removedCliques; + }; + // Remove each variable and its subtrees - for(Key j: leafKeys) { - if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree + for (Key j : leafKeys) { + if (!leafKeysRemoved.exists(j)) { // If the index was not already removed + // by removing another subtree // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; - while(!clique->parent_._empty()) - { - // Check if parent contains a marginalized leaf variable. Only need to check the first - // variable because it is the closest to the leaves. + while (!clique->parent_._empty()) { + // Check if parent contains a marginalized leaf variable. Only need to + // check the first variable because it is the closest to the leaves. sharedClique parent = clique->parent(); - if(leafKeys.exists(parent->conditional()->front())) + if (leafKeys.exists(parent->conditional()->front())) clique = parent; else break; @@ -788,39 +868,28 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // See if we should remove the whole clique bool marginalizeEntireClique = true; - for(Key frontal: clique->conditional()->frontals()) { - if(!leafKeys.exists(frontal)) { + for (Key frontal : clique->conditional()->frontals()) { + if (!leafKeys.exists(frontal)) { marginalizeEntireClique = false; - break; } } + break; + } + } // Remove either the whole clique or part of it - if(marginalizeEntireClique) { - // Remove the whole clique and its subtree, and keep the marginal factor. - GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); + if (marginalizeEntireClique) { + // Remove the whole clique and its subtree, and keep the marginal + // factor. + auto marginalFactor = clique->cachedFactor(); // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the // parent of this clique. - marginalFactors[clique->parent()->conditional()->front()].push_back(marginalFactor); + marginalFactors[clique->parent()->conditional()->front()].push_back( + marginalFactor); // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. - const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques - for(const sharedClique& removedClique: removedCliques) { - marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { - // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); - - // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); - } - } - } - else { + trackingRemoveSubtree(clique); + } else { // Reeliminate the current clique and the marginals from its children, // then keep only the marginal on the non-marginalized variables. We // get the childrens' marginals from any existing children, plus @@ -831,181 +900,189 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, GaussianFactorGraph graph; KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; - for(const sharedClique& child: clique->children) { + for (const sharedClique& child : clique->children) { // Remove subtree if child depends on any marginalized keys - for(Key parent: child->conditional()->parents()) { - if(leafKeys.exists(parent)) { + for (Key parent : child->conditional()->parents()) { + if (leafKeys.exists(parent)) { subtreesToRemove.push_back(child); - graph.push_back(child->cachedFactor()); // Add child marginal + graph.push_back(child->cachedFactor()); // Add child marginal break; } } } Cliques childrenRemoved; - for(const sharedClique& childToRemove: subtreesToRemove) { - const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques - childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); - for(const sharedClique& removedClique: removedCliques) { - marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { - // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + for (const sharedClique& subtree : subtreesToRemove) { + const Cliques removed = trackingRemoveSubtree(subtree); + childrenRemoved.insert(childrenRemoved.end(), removed.begin(), + removed.end()); + } - // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + // Add the factors that are pulled into the current clique by the + // marginalized variables. These are the factors that involve + // *marginalized* frontal variables in this clique but do not involve + // frontal variables of any of its children. + // TODO(dellaert): reuse cached linear factors + KeySet factorsFromMarginalizedInClique_step1; + for (Key frontal : clique->conditional()->frontals()) { + if (leafKeys.exists(frontal)) + factorsFromMarginalizedInClique_step1.insert( + variableIndex_[frontal].begin(), variableIndex_[frontal].end()); + } + // Remove any factors in subtrees that we're removing at this step + for (const sharedClique& removedChild : childrenRemoved) { + for (Key indexInClique : removedChild->conditional()->frontals()) { + for (Key factorInvolving : variableIndex_[indexInClique]) { + factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } - - // Add the factors that are pulled into the current clique by the marginalized variables. - // These are the factors that involve *marginalized* frontal variables in this clique - // but do not involve frontal variables of any of its children. - // TODO: reuse cached linear factors - KeySet factorsFromMarginalizedInClique_step1; - for(Key frontal: clique->conditional()->frontals()) { - if(leafKeys.exists(frontal)) - factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } - // Remove any factors in subtrees that we're removing at this step - for(const sharedClique& removedChild: childrenRemoved) { - for(Key indexInClique: removedChild->conditional()->frontals()) { - for(Key factorInvolving: variableIndex_[indexInClique]) { - factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } // Create factor graph from factor indices - for(size_t i: factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } + for (size_t i : factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + } - // Reeliminate the linear graph to get the marginal and discard the conditional - const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + // Reeliminate the linear graph to get the marginal and discard the + // conditional + auto cg = clique->conditional(); + const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals()); FastVector cliqueFrontalsToEliminate; - std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), - std::back_inserter(cliqueFrontalsToEliminate)); - pair eliminationResult1 = - params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate)); + std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), + leafKeys.begin(), leafKeys.end(), + std::back_inserter(cliqueFrontalsToEliminate)); + auto eliminationResult1 = params_.getEliminationFunction()( + graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal - if(eliminationResult1.second) - marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second); + if (eliminationResult1.second) + marginalFactors[cg->front()].push_back(eliminationResult1.second); // Split the current clique // Find the position of the last leaf key in this clique DenseIndex nToRemove = 0; - while(leafKeys.exists(clique->conditional()->keys()[nToRemove])) - ++ nToRemove; + while (leafKeys.exists(cg->keys()[nToRemove])) ++nToRemove; // Make the clique's matrix appear as a subset - const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove); - clique->conditional()->matrixObject().firstBlock() = nToRemove; - clique->conditional()->matrixObject().rowStart() = dimToRemove; + const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove); + cg->matrixObject().firstBlock() = nToRemove; + cg->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique - FastVector originalKeys; originalKeys.swap(clique->conditional()->keys()); - clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); - clique->conditional()->nrFrontals() -= nToRemove; + FastVector originalKeys; + originalKeys.swap(cg->keys()); + cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + cg->nrFrontals() -= nToRemove; - // Add to factors to remove factors involved in frontals of current clique - for(Key frontal: cliqueFrontalsToEliminate) - { - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + // Add to factorIndicesToRemove any factors involved in frontals of + // current clique + for (Key frontal : cliqueFrontalsToEliminate) { + const auto& involved = variableIndex_[frontal]; + factorIndicesToRemove.insert(involved.begin(), involved.end()); } // Add removed keys - leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); + leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), + cliqueFrontalsToEliminate.end()); } } } - // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures + // At this point we have updated the BayesTree, now update the remaining iSAM2 + // data structures // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; - typedef pair > Key_Factors; - for(const Key_Factors& key_factors: marginalFactors) { - for(const GaussianFactor::shared_ptr& factor: key_factors.second) { - if(factor) { + for (const auto& key_factors : marginalFactors) { + for (const auto& factor : key_factors.second) { + if (factor) { factorsToAdd.push_back(factor); - if(marginalFactorsIndices) + if (marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back(boost::make_shared( - factor)); - if(params_.cacheLinearizedFactors) - linearFactors_.push_back(factor); - for(Key factorKey: *factor) { - fixedVariables_.insert(factorKey); } + nonlinearFactors_.push_back( + boost::make_shared(factor)); + if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); + for (Key factorKey : *factor) { + fixedVariables_.insert(factorKey); + } } } } - variableIndex_.augment(factorsToAdd); // Augment the variable index + variableIndex_.augment(factorsToAdd); // Augment the variable index - // Remove the factors to remove that have been summarized in the newly-added marginal factors + // Remove the factors to remove that have been summarized in the newly-added + // marginal factors NonlinearFactorGraph removedFactors; - for(size_t i: factorIndicesToRemove) { + for (size_t i : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(i); + if (params_.cacheLinearizedFactors) linearFactors_.remove(i); } - variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); + variableIndex_.remove(factorIndicesToRemove.begin(), + factorIndicesToRemove.end(), removedFactors); - if(deletedFactorsIndices) - deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); + if (deletedFactorsIndices) + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), + factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, nodes_, fixedVariables_); + removeVariables(KeySet(leafKeys.begin(), leafKeys.end())); } /* ************************************************************************* */ -void ISAM2::updateDelta(bool forceFullSolve) const -{ +void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { // If using Gauss-Newton, update with wildfireThreshold const ISAM2GaussNewtonParams& gaussNewtonParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + } else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; // Do one Dogleg iteration gttic(Dogleg_Iterate); // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); gttoc(Wildfire_update); // Compute steepest descent step - const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient - Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd - const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point + const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient + Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = Impl::ComputeGradientSearch( + gradAtZero, RgProd_); // Compute gradient search point - // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ + // Clear replaced keys mask because now we've updated deltaNewton_ and + // RgProd_ deltaReplacedMask_.clear(); // Compute dogleg point - DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, - theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); + DoglegOptimizerImpl::IterationResult doglegResult( + DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, + *this, nonlinearFactors_, theta_, nonlinearFactors_.error(theta_), + doglegParams.verbose)); gttoc(Dogleg_Iterate); gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + delta_ = + doglegResult + .dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } } @@ -1027,60 +1104,61 @@ const Value& ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - updateDelta(true); // Force full solve when updating delta_ + updateDelta(true); // Force full solve when updating delta_ return theta_.retract(delta_); } /* ************************************************************************* */ Matrix ISAM2::marginalCovariance(Key key) const { - return marginalFactor(key, params_.getEliminationFunction())->information().inverse(); + return marginalFactor(key, params_.getEliminationFunction()) + ->information() + .inverse(); } /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { - if(!deltaReplacedMask_.empty()) - updateDelta(); + if (!deltaReplacedMask_.empty()) updateDelta(); return delta_; } /* ************************************************************************* */ -double ISAM2::error(const VectorValues& x) const -{ +double ISAM2::error(const VectorValues& x) const { return GaussianFactorGraph(*this).error(x); } /* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { +static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, + VectorValues* g) { // Loop through variables in each clique, adding contributions DenseIndex variablePosition = 0; - for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + for (GaussianConditional::const_iterator jit = root->conditional()->begin(); + jit != root->conditional()->end(); ++jit) { const DenseIndex dim = root->conditional()->getDim(jit); - pair pos_ins = - g.tryInsert(*jit, root->gradientContribution().segment(variablePosition, dim)); - if(!pos_ins.second) - pos_ins.first->second += root->gradientContribution().segment(variablePosition, dim); + pair pos_ins = g->tryInsert( + *jit, root->gradientContribution().segment(variablePosition, dim)); + if (!pos_ins.second) + pos_ins.first->second += + root->gradientContribution().segment(variablePosition, dim); variablePosition += dim; } // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - for(const sharedClique& child: root->children) { + for (const sharedClique& child : root->children) { gradientAtZeroTreeAdder(child, g); } } /* ************************************************************************* */ -VectorValues ISAM2::gradientAtZero() const -{ +VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; // Sum up contributions for each clique - for(const ISAM2::sharedClique& root: this->roots()) - gradientAtZeroTreeAdder(root, g); + for (const ISAM2::sharedClique& root : this->roots()) + gradientAtZeroTreeAdder(root, &g); return g; } -} -/// namespace gtsam +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 114c04018..5d448d786 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -11,438 +11,47 @@ /** * @file ISAM2.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ // \callgraph #pragma once +#include +#include +#include #include -#include #include -#include +#include namespace gtsam { /** * @addtogroup ISAM2 - * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or - * ISAM2DoglegParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2GaussNewtonParams { - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) - - /** Specify parameters as constructor arguments */ - ISAM2GaussNewtonParams( - double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold - ) : wildfireThreshold(_wildfireThreshold) {} - - void print(const std::string str = "") const { - std::cout << str << "type: ISAM2GaussNewtonParams\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout.flush(); - } - - double getWildfireThreshold() const { return wildfireThreshold; } - void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; } -}; - -/** - * @addtogroup ISAM2 - * Parameters for ISAM2 using Dogleg optimization. Either this class or - * ISAM2GaussNewtonParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2DoglegParams { - double initialDelta; ///< The initial trust region radius for Dogleg - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 1e-5) - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode - bool verbose; ///< Whether Dogleg prints iteration and convergence information - - /** Specify parameters as constructor arguments */ - ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta - double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams::verbose - ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold), - adaptationMode(_adaptationMode), verbose(_verbose) {} - - void print(const std::string str = "") const { - std::cout << str << "type: ISAM2DoglegParams\n"; - std::cout << str << "initialDelta: " << initialDelta << "\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout << str << "adaptationMode: " << adaptationModeTranslator(adaptationMode) << "\n"; - std::cout.flush(); - } - - double getInitialDelta() const { return initialDelta; } - double getWildfireThreshold() const { return wildfireThreshold; } - std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); }; - bool isVerbose() const { return verbose; }; - - void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; } - void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; } - void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); } - void setVerbose(bool verbose) { this->verbose = verbose; }; - - std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const; - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const; -}; - -/** - * @addtogroup ISAM2 - * Parameters for the ISAM2 algorithm. Default parameter values are listed below. - */ -typedef FastMap ISAM2ThresholdMap; -typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; -struct GTSAM_EXPORT ISAM2Params { - typedef boost::variant OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams - typedef boost::variant > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds - - /** Optimization parameters, this both selects the nonlinear optimization - * method and specifies its parameters, either ISAM2GaussNewtonParams or - * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used - * with the specified parameters, and in the latter Powell's dog-leg - * algorithm will be used with the specified parameters. - */ - OptimizationParams optimizationParams; - - /** Only relinearize variables whose linear delta magnitude is greater than - * this threshold (default: 0.1). If this is a FastMap instead - * of a double, then the threshold is specified for each dimension of each - * variable type. This parameter then maps from a character indicating the - * variable type to a Vector of thresholds for each dimension of that - * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, - * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate - * entries would be added with: - * \code - FastMap thresholds; - thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold - params.relinearizeThreshold = thresholds; - \endcode - */ - RelinearizationThreshold relinearizeThreshold; - - int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10) - - bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true) - - bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update() - - enum Factorization { CHOLESKY, QR }; - /** Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY). - * Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when - * uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is - * slower but more numerically stable in poorly-conditioned problems. We suggest using the default of Cholesky - * unless gtsam sometimes throws IndefiniteLinearSystemException when your problem's Hessian is actually positive - * definite. For positive definite problems, numerical error accumulation can cause the problem to become - * numerically negative or indefinite as solving proceeds, especially when using Cholesky. - */ - Factorization factorization; - - /** Whether to cache linear factors (default: true). - * This can improve performance if linearization is expensive, but can hurt - * performance if linearization is very cleap due to computation to look up - * additional keys. - */ - bool cacheLinearizedFactors; - - KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) - - bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false) - - /** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false). - * This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate - * significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance - * is desired over correctness. Use with caution. - */ - bool enablePartialRelinearizationCheck; - - /// 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 cost of - /// having to search for slots every time a factor is added. - bool findUnusedFactorSlots; - - /** Specify parameters as constructor arguments */ - ISAM2Params( - OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams - RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold - int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip - bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization - bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError - Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization - bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors - const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), - relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), - evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), enablePartialRelinearizationCheck(false), - findUnusedFactorSlots(false) {} - - /// print iSAM2 parameters - void print(const std::string& str = "") const { - std::cout << str << "\n"; - if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print("optimizationParams: "); - else if(optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print("optimizationParams: "); - else - std::cout << "optimizationParams: " << "{unknown type}" << "\n"; - if(relinearizeThreshold.type() == typeid(double)) - std::cout << "relinearizeThreshold: " << boost::get(relinearizeThreshold) << "\n"; - else - { - std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; - for(const ISAM2ThresholdMapValue& value: boost::get(relinearizeThreshold)) { - std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; - } - } - std::cout << "relinearizeSkip: " << relinearizeSkip << "\n"; - std::cout << "enableRelinearization: " << enableRelinearization << "\n"; - std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n"; - std::cout << "factorization: " << factorizationTranslator(factorization) << "\n"; - std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n"; - std::cout << "enableDetailedResults: " << enableDetailedResults << "\n"; - std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n"; - std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n"; - std::cout.flush(); - } - - /// @name Getters and Setters for all properties - /// @{ - - OptimizationParams getOptimizationParams() const { return this->optimizationParams; } - RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } - int getRelinearizeSkip() const { return relinearizeSkip; } - bool isEnableRelinearization() const { return enableRelinearization; } - bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } - std::string getFactorization() const { return factorizationTranslator(factorization); } - bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } - KeyFormatter getKeyFormatter() const { return keyFormatter; } - bool isEnableDetailedResults() const { return enableDetailedResults; } - bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; } - - void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; } - void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; } - void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; } - void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; } - void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; } - void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); } - void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; } - void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; } - void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - return factorization == CHOLESKY - ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky - : (GaussianFactorGraph::Eliminate)EliminateQR; - } - - /// @} - - /// @name Some utilities - /// @{ - - static Factorization factorizationTranslator(const std::string& str); - static std::string factorizationTranslator(const Factorization& value); - - /// @} -}; - -typedef FastVector FactorIndices; - -/** - * @addtogroup ISAM2 - * This struct is returned from ISAM2::update() and contains information about - * the update that is useful for determining whether the solution is - * converging, and about how much work was required for the update. See member - * variables for details and information about each entry. - */ -struct GTSAM_EXPORT ISAM2Result { - /** The nonlinear error of all of the factors, \a including new factors and - * variables added during the current call to ISAM2::update(). This error is - * calculated using the following variable values: - * \li Pre-existing variables will be evaluated by combining their - * linearization point before this call to update, with their partial linear - * delta, as computed by ISAM2::calculateEstimate(). - * \li New variables will be evaluated at their initialization points passed - * into the current call to update. - * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError - * is set to \c true, because there is some cost to this computation. - */ - boost::optional errorBefore; - - /** The nonlinear error of all of the factors computed after the current - * update, meaning that variables above the relinearization threshold - * (ISAM2Params::relinearizeThreshold) have been relinearized and new - * variables have undergone one linear update. Variable values are - * again computed by combining their linearization points with their - * partial linear deltas, by ISAM2::calculateEstimate(). - * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError - * is set to \c true, because there is some cost to this computation. - */ - boost::optional errorAfter; - - /** The number of variables that were relinearized because their linear - * deltas exceeded the reslinearization threshold - * (ISAM2Params::relinearizeThreshold), combined with any additional - * variables that had to be relinearized because they were involved in - * the same factor as a variable above the relinearization threshold. - * On steps where no relinearization is considered - * (see ISAM2Params::relinearizeSkip), this count will be zero. - */ - size_t variablesRelinearized; - - /** The number of variables that were reeliminated as parts of the Bayes' - * Tree were recalculated, due to new factors. When loop closures occur, - * this count will be large as the new loop-closing factors will tend to - * involve variables far away from the root, and everything up to the root - * will be reeliminated. - */ - size_t variablesReeliminated; - - /** The number of factors that were included in reelimination of the Bayes' tree. */ - size_t factorsRecalculated; - - /** The number of cliques in the Bayes' Tree */ - size_t cliques; - - /** The indices of the newly-added factors, in 1-to-1 correspondence with the - * factors passed as \c newFactors to ISAM2::update(). These indices may be - * used later to refer to the factors in order to remove them. - */ - FactorIndices newFactorsIndices; - - /** A struct holding detailed results, which must be enabled with - * ISAM2Params::enableDetailedResults. - */ - struct DetailedResults { - /** The status of a single variable, this struct is stored in - * DetailedResults::variableStatus */ - struct VariableStatus { - /** Whether the variable was just reeliminated, due to being relinearized, - * observed, new, or on the path up to the root clique from another - * reeliminated variable. */ - bool isReeliminated; - bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold - bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold - bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement. - bool isObserved; ///< Whether the variable was just involved in new factors - bool isNew; ///< Whether the variable itself was just added - bool inRootClique; ///< Whether the variable is in the root clique - VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false), - isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {} - }; - - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; - }; - - /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See - * Detail for information about the results data stored here. */ - boost::optional detail; - - - void print(const std::string str = "") const { - std::cout << str << " Reelimintated: " << variablesReeliminated << " Relinearized: " << variablesRelinearized << " Cliques: " << cliques << std::endl; - } - - /** Getters and Setters */ - size_t getVariablesRelinearized() const { return variablesRelinearized; }; - size_t getVariablesReeliminated() const { return variablesReeliminated; }; - size_t getCliques() const { return cliques; }; -}; - -/** - * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution - * TODO: more documentation - */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase -{ -public: - typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef GaussianConditional ConditionalType; - typedef ConditionalType::shared_ptr sharedConditional; - - Base::FactorType::shared_ptr cachedFactor_; - Vector gradientContribution_; - FastMap solnPointers_; - - /// Default constructor - ISAM2Clique() : Base() {} - - /// Copy constructor, does *not* copy solution pointers as these are invalid in different trees. - ISAM2Clique(const ISAM2Clique& other) : - Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {} - - /// Assignment operator, does *not* copy solution pointers as these are invalid in different trees. - ISAM2Clique& operator=(const ISAM2Clique& other) - { - Base::operator=(other); - cachedFactor_ = other.cachedFactor_; - gradientContribution_ = other.gradientContribution_; - return *this; - } - - /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult); - - /** Access the cached factor */ - Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - - /** Access the gradient contribution */ - const Vector& gradientContribution() const { return gradientContribution_; } - - bool equals(const This& other, double tol=1e-9) const; - - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(cachedFactor_); - ar & BOOST_SERIALIZATION_NVP(gradientContribution_); - } -}; // \struct ISAM2Clique - -/** - * @addtogroup ISAM2 - * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. + * Implementation of the full ISAM2 algorithm for incremental nonlinear + * optimization. * - * The typical cycle of using this class to create an instance by providing ISAM2Params - * to the constructor, then add measurements and variables as they arrive using the update() - * method. At any time, calculateEstimate() may be called to obtain the current - * estimate of all variables. + * The typical cycle of using this class to create an instance by providing + * ISAM2Params to the constructor, then add measurements and variables as they + * arrive using the update() method. At any time, calculateEstimate() may be + * called to obtain the current estimate of all variables. * */ -class GTSAM_EXPORT ISAM2: public BayesTree { - -protected: - +class GTSAM_EXPORT ISAM2 : public BayesTree { + protected: /** The current linearization point */ Values theta_; - /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ + /** VariableIndex lets us look up factors by involved variable and keeps track + * of dimensions */ VariableIndex variableIndex_; - /** The linear delta from the last linear solution, an update to the estimate in theta + /** The linear delta from the last linear solution, an update to the estimate + * in theta * * This is \c mutable because it is a "cached" variable - it is not updated * until either requested with getDelta() or calculateEstimate(), or needed @@ -450,8 +59,10 @@ protected: */ mutable VectorValues delta_; - mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update - mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally + mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores + // the Gauss-Newton update + mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and + // is updated incrementally /** A cumulative mask for the variables that were replaced and have not yet * been updated in the linear solution delta_, this is only used internally, @@ -461,9 +72,11 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet + deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way - /** All original nonlinear factors are stored here to use during relinearization */ + /** All original nonlinear factors are stored here to use during + * relinearization */ NonlinearFactorGraph nonlinearFactors_; /** The current linear factors, which are only updated as needed */ @@ -479,20 +92,21 @@ protected: * variables and thus cannot have their linearization points changed. */ KeySet fixedVariables_; - int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization + int update_count_; ///< Counter incremented every update(), used to determine + ///< periodic relinearization -public: - - typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - typedef Base::Clique Clique; ///< A clique - typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + public: + typedef ISAM2 This; ///< This class + typedef BayesTree Base; ///< The BayesTree base class + typedef Base::Clique Clique; ///< A clique + typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef Base::Cliques Cliques; ///< List of Clique typedef from base class /** Create an empty ISAM2 instance */ - ISAM2(const ISAM2Params& params); + explicit ISAM2(const ISAM2Params& params); - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + /** Create an empty ISAM2 instance using the default set of parameters (see + * ISAM2Params) */ ISAM2(); /** default virtual destructor */ @@ -513,26 +127,31 @@ public: * thresholds. * * @param newFactors The new factors to be added to the system - * @param newTheta Initialization points for new variables to be added to the system. - * You must include here all new variables occuring in newFactors (which were not already - * in the system). There must not be any variables here that do not occur in newFactors, - * and additionally, variables that were already in the system must not be included here. + * @param newTheta Initialization points for new variables to be added to the + * system. You must include here all new variables occuring in newFactors + * (which were not already in the system). There must not be any variables + * here that do not occur in newFactors, and additionally, variables that were + * already in the system must not be included here. * @param removeFactorIndices Indices of factors to remove from system - * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently - * large (Params::relinearizeThreshold), regardless of the relinearization interval - * (Params::relinearizeSkip). - * @param constrainedKeys is an optional map of keys to group labels, such that a variable can - * be constrained to a particular grouping in the BayesTree - * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization - * point, regardless of the size of the linear delta - * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless - * of the size of the linear delta. This allows the provided keys to be reordered. + * @param force_relinearize Relinearize any variables whose delta magnitude is + * sufficiently large (Params::relinearizeThreshold), regardless of the + * relinearization interval (Params::relinearizeSkip). + * @param constrainedKeys is an optional map of keys to group labels, such + * that a variable can be constrained to a particular grouping in the + * BayesTree + * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will + * hold at a constant linearization point, regardless of the size of the + * linear delta + * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will + * re-eliminate, regardless of the size of the linear delta. This allows the + * provided keys to be reordered. * @return An ISAM2Result struct containing information about the update */ - virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + virtual ISAM2Result update( + const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FactorIndices& removeFactorIndices = FactorIndices(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, bool force_relinearize = false); @@ -542,48 +161,54 @@ public: * requested to be marginalized. Marginalization leaves a linear * approximation of the marginal in the system, and the linearization points * of any variables involved in this linear marginal become fixed. The set - * fixed variables will include any key involved with the marginalized variables - * in the original factors, and possibly additional ones due to fill-in. + * fixed variables will include any key involved with the marginalized + * variables in the original factors, and possibly additional ones due to + * fill-in. * - * If provided, 'marginalFactorsIndices' will be augmented with the factor graph - * indices of the marginal factors added during the 'marginalizeLeaves' call + * If provided, 'marginalFactorsIndices' will be augmented with the factor + * graph indices of the marginal factors added during the 'marginalizeLeaves' + * call * - * If provided, 'deletedFactorsIndices' will be augmented with the factor graph - * indices of any factor that was removed during the 'marginalizeLeaves' call + * If provided, 'deletedFactorsIndices' will be augmented with the factor + * graph indices of any factor that was removed during the 'marginalizeLeaves' + * call */ - void marginalizeLeaves(const FastList& leafKeys, - boost::optional marginalFactorsIndices = boost::none, - boost::optional deletedFactorsIndices = boost::none); + void marginalizeLeaves( + const FastList& leafKeys, + boost::optional marginalFactorsIndices = boost::none, + boost::optional deletedFactorsIndices = boost::none); /// Access the current linearization point - const Values& getLinearizationPoint() const { - return theta_; - } + const Values& getLinearizationPoint() const { return theta_; } /// Check whether variable with given key exists in linearization point - bool valueExists(Key key) const { - return theta_.exists(key); - } + bool valueExists(Key key) const { return theta_.exists(key); } - /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. If only - * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + /** Compute an estimate from the incomplete linear delta computed during the + * last update. This delta is incomplete because it was not updated below + * wildfire_threshold. If only a single variable is needed, it is faster to + * call calculateEstimate(const KEY&). */ Values calculateEstimate() const; - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. * @param key * @return */ - template - VALUE calculateEstimate(Key key) const; + template + VALUE calculateEstimate(Key key) const { + const Vector& delta = getDelta()[key]; + return traits::Retract(theta_.at(key), delta); + } - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. This is a non-templated version - * that returns a Value base class for use with the MATLAB wrapper. + + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. + * This is a non-templated version that returns a Value base class for use + * with the MATLAB wrapper. * @param key * @return */ @@ -598,7 +223,8 @@ public: /** Internal implementation functions */ struct Impl; - /** Compute an estimate using a complete delta computed by a full back-substitution. + /** Compute an estimate using a complete delta computed by a full + * back-substitution. */ Values calculateBestEstimate() const; @@ -609,7 +235,9 @@ public: double error(const VectorValues& x) const; /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { + return nonlinearFactors_; + } /** Access the nonlinear variable index */ const VariableIndex& getVariableIndex() const { return variableIndex_; } @@ -628,56 +256,60 @@ public: /** prints out clique statistics */ void printStats() const { getCliqueData().getStats().print(); } - - /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d - * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also - * gradient(const GaussianBayesNet&, const VectorValues&). + + /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert + * \Sigma^{-1} R x - d \right\Vert^2 \f$, centered around zero. The gradient + * about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, + * const VectorValues&). * * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - + /// @} -protected: + protected: + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param deltaNewton + * @param RgProd + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + void addVariables(const Values& newTheta); + + /** + * Remove variables from the ISAM2 system. + */ + void removeVariables(const KeySet& unusedKeys); + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c mask. Values are expmapped in-place. + * \param mask Mask on linear indices, only \c true entries are expmapped + */ + void expmapMasked(const KeySet& mask); FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const; + GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); + + virtual boost::shared_ptr recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result* result); - virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; - -}; // ISAM2 +}; // ISAM2 /// traits -template<> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; -/// Optimize the BayesTree, starting from the root. -/// @param replaced Needs to contain -/// all variables that are contained in the top of the Bayes tree that has been -/// redone. -/// @param delta The current solution, an offset from the linearization -/// point. -/// @param threshold The maximum change against the PREVIOUS delta for -/// non-replaced variables that can be ignored, ie. the old delta entry is kept -/// and recursive backsubstitution might eventually stop if none of the changed -/// variables are contained in the subtree. -/// @return The number of variables that were solved for -template -size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); +} // namespace gtsam -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); - -/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) -template -int calculate_nnz(const boost::shared_ptr& clique); - -} /// namespace gtsam - -#include #include diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp new file mode 100644 index 000000000..c55ca7959 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -0,0 +1,325 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Clique.cpp + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +// Instantiate base class +template class BayesTreeCliqueBase; + +/* ************************************************************************* */ +void ISAM2Clique::setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult) { + conditional_ = eliminationResult.first; + cachedFactor_ = eliminationResult.second; + // Compute gradient contribution + gradientContribution_.resize(conditional_->cols() - 1); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed + // reasons + gradientContribution_ << -conditional_->get_R().transpose() * + conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); +} + +/* ************************************************************************* */ +bool ISAM2Clique::equals(const This& other, double tol) const { + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) || + (cachedFactor_ && other.cachedFactor_ && + cachedFactor_->equals(*other.cachedFactor_, tol))); +} + +/* ************************************************************************* */ +void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { + Base::print(s, formatter); + if (cachedFactor_) + cachedFactor_->print(s + "Cached: ", formatter); + else + cout << s << "Cached empty" << endl; + if (gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); +} + +/* ************************************************************************* */ +bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool dirty = replaced.exists(conditional_->frontals().front()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (Key frontal : conditional_->frontals()) { + assert(dirty == replaced.exists(frontal)); + } +#endif + + // If not, then has one of the separator variables changed significantly? + if (!dirty) { + for (Key parent : conditional_->parents()) { + if (changed.exists(parent)) { + dirty = true; + break; + } + } + } + return dirty; +} + +/* ************************************************************************* */ +/** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ +void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2Clique::shared_ptr parent = parent_.lock(); + if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } +#else + delta->update(conditional_->solve(*delta)); +#endif +} + +/* ************************************************************************* */ +bool ISAM2Clique::valuesChanged(const KeySet& replaced, + const Vector& originalValues, + const VectorValues& delta, + double threshold) const { + auto frontals = conditional_->frontals(); + if (replaced.exists(frontals.front())) return true; + auto diff = originalValues - delta.vector(frontals); + return diff.lpNorm() >= threshold; +} + +/* ************************************************************************* */ +/// Set changed flag for each frontal variable +void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { + for (Key frontal : conditional_->frontals()) { + changed->insert(frontal); + } +} + +/* ************************************************************************* */ +void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const { + size_t pos = 0; + for (Key frontal : conditional_->frontals()) { + auto v = delta->at(frontal); + v = originalValues.segment(pos, v.size()); + pos += v.size(); + } +} + +/* ************************************************************************* */ +// Note: not being used right now in favor of non-recursive version below. +void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + if (isDirty(replaced, *changed)) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + + // Recurse to children + for (const auto& child : children) { + child->optimizeWildfire(replaced, threshold, changed, delta, count); + } + } +} + +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + +/* ************************************************************************* */ +bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + bool dirty = isDirty(replaced, *changed); + if (dirty) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + } + + return dirty; +} + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& keys, + VectorValues* delta) { + KeySet changed; + size_t count = 0; + + if (root) { + std::stack travStack; + travStack.push(root); + ISAM2Clique::shared_ptr currentNode = root; + while (!travStack.empty()) { + currentNode = travStack.top(); + travStack.pop(); + bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, + delta, &count); + if (dirty) { + for (const auto& child : currentNode->children) { + travStack.push(child); + } + } + } + } + + return count; +} + +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + +/* ************************************************************************* */ +void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { + static const bool debug = false; + // does the separator contain any of the variables? + bool found = false; + for (Key key : conditional()->parents()) { + if (markedMask.exists(key)) { + found = true; + break; + } + } + if (found) { + // then add this clique + keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); + if (debug) print("Key(s) marked in clique "); + if (debug) cout << "so marking key " << conditional()->front() << endl; + } + for (const auto& child : children) { + child->findAll(markedMask, keys); + } +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h new file mode 100644 index 000000000..3c53e3d72 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Clique.h + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Specialized Clique structure for ISAM2, incorporating caching and gradient + * contribution + * TODO: more documentation + */ +class GTSAM_EXPORT ISAM2Clique + : public BayesTreeCliqueBase { + public: + typedef ISAM2Clique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef GaussianConditional ConditionalType; + typedef ConditionalType::shared_ptr sharedConditional; + + Base::FactorType::shared_ptr cachedFactor_; + Vector gradientContribution_; +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + mutable FastMap solnPointers_; +#endif + + /// Default constructor + ISAM2Clique() : Base() {} + + /// Copy constructor, does *not* copy solution pointers as these are invalid + /// in different trees. + ISAM2Clique(const ISAM2Clique& other) + : Base(other), + cachedFactor_(other.cachedFactor_), + gradientContribution_(other.gradientContribution_) {} + + /// Assignment operator, does *not* copy solution pointers as these are + /// invalid in different trees. + ISAM2Clique& operator=(const ISAM2Clique& other) { + Base::operator=(other); + cachedFactor_ = other.cachedFactor_; + gradientContribution_ = other.gradientContribution_; + return *this; + } + + /// Overridden to also store the remaining factor and gradient contribution + void setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult); + + /** Access the cached factor */ + Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } + + bool equals(const This& other, double tol = 1e-9) const; + + /** print this node */ + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + void optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + bool optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + /** + * Starting from the root, add up entries of frontal and conditional matrices + * of each conditional + */ + void nnz_internal(size_t* result) const; + size_t calculate_nnz() const; + + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + void findAll(const KeySet& markedMask, KeySet* keys) const; + + private: + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ + void fastBackSubstitute(VectorValues* delta) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, const Vector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const; + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(cachedFactor_); + ar& BOOST_SERIALIZATION_NVP(gradientContribution_); + } +}; // \struct ISAM2Clique + +/** + * Optimize the BayesTree, starting from the root. + * @param threshold The maximum change against the PREVIOUS delta for + * non-replaced variables that can be ignored, ie. the old delta entry is kept + * and recursive backsubstitution might eventually stop if none of the changed + * variables are contained in the subtree. + * @param replaced Needs to contain all variables that are contained in the top + * of the Bayes tree that has been redone. + * @return The number of variables that were solved for. + * @param delta The current solution, an offset from the linearization point. + */ +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& replaced, VectorValues* delta); + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& replaced, + VectorValues* delta); + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.cpp b/gtsam/nonlinear/ISAM2Params.cpp new file mode 100644 index 000000000..c768ce427 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Params.cpp + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string ISAM2DoglegParams::adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const { + string s; + switch (adaptationMode) { + case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: + s = "SEARCH_EACH_ITERATION"; + break; + case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: + s = "ONE_STEP_PER_ITERATION"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +/* ************************************************************************* */ +DoglegOptimizerImpl::TrustRegionAdaptationMode +ISAM2DoglegParams::adaptationModeTranslator( + const string& adaptationMode) const { + string s = adaptationMode; + boost::algorithm::to_upper(s); + if (s == "SEARCH_EACH_ITERATION") + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; + if (s == "ONE_STEP_PER_ITERATION") + return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; + + /* default is SEARCH_EACH_ITERATION */ + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; +} + +/* ************************************************************************* */ +ISAM2Params::Factorization ISAM2Params::factorizationTranslator( + const string& str) { + string s = str; + boost::algorithm::to_upper(s); + if (s == "QR") return ISAM2Params::QR; + if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; + + /* default is CHOLESKY */ + return ISAM2Params::CHOLESKY; +} + +/* ************************************************************************* */ +string ISAM2Params::factorizationTranslator( + const ISAM2Params::Factorization& value) { + string s; + switch (value) { + case ISAM2Params::QR: + s = "QR"; + break; + case ISAM2Params::CHOLESKY: + s = "CHOLESKY"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h new file mode 100644 index 000000000..afddd1f8e --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.h @@ -0,0 +1,365 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Params.h + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or + * ISAM2DoglegParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2GaussNewtonParams { + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 0.001) + + /** Specify parameters as constructor arguments */ + ISAM2GaussNewtonParams( + double _wildfireThreshold = + 0.001 ///< see ISAM2GaussNewtonParams public variables, + ///< ISAM2GaussNewtonParams::wildfireThreshold + ) + : wildfireThreshold(_wildfireThreshold) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2GaussNewtonParams\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout.flush(); + } + + double getWildfireThreshold() const { return wildfireThreshold; } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } +}; + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Dogleg optimization. Either this class or + * ISAM2GaussNewtonParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2DoglegParams { + double initialDelta; ///< The initial trust region radius for Dogleg + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 1e-5) + DoglegOptimizerImpl::TrustRegionAdaptationMode + adaptationMode; ///< See description in + ///< DoglegOptimizerImpl::TrustRegionAdaptationMode + bool + verbose; ///< Whether Dogleg prints iteration and convergence information + + /** Specify parameters as constructor arguments */ + ISAM2DoglegParams( + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = + 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = + DoglegOptimizerImpl:: + SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) + : initialDelta(_initialDelta), + wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), + verbose(_verbose) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2DoglegParams\n"; + cout << str << "initialDelta: " << initialDelta << "\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout << str + << "adaptationMode: " << adaptationModeTranslator(adaptationMode) + << "\n"; + cout.flush(); + } + + double getInitialDelta() const { return initialDelta; } + double getWildfireThreshold() const { return wildfireThreshold; } + std::string getAdaptationMode() const { + return adaptationModeTranslator(adaptationMode); + } + bool isVerbose() const { return verbose; } + void setInitialDelta(double initialDelta) { + this->initialDelta = initialDelta; + } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } + void setAdaptationMode(const std::string& adaptationMode) { + this->adaptationMode = adaptationModeTranslator(adaptationMode); + } + void setVerbose(bool verbose) { this->verbose = verbose; } + + std::string adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const; + DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( + const std::string& adaptationMode) const; +}; + +/** + * @addtogroup ISAM2 + * Parameters for the ISAM2 algorithm. Default parameter values are listed + * below. + */ +typedef FastMap ISAM2ThresholdMap; +typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; +struct GTSAM_EXPORT ISAM2Params { + typedef boost::variant + OptimizationParams; ///< Either ISAM2GaussNewtonParams or + ///< ISAM2DoglegParams + typedef boost::variant > + RelinearizationThreshold; ///< Either a constant relinearization + ///< threshold or a per-variable-type set of + ///< thresholds + + /** Optimization parameters, this both selects the nonlinear optimization + * method and specifies its parameters, either ISAM2GaussNewtonParams or + * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used + * with the specified parameters, and in the latter Powell's dog-leg + * algorithm will be used with the specified parameters. + */ + OptimizationParams optimizationParams; + + /** Only relinearize variables whose linear delta magnitude is greater than + * this threshold (default: 0.1). If this is a FastMap instead + * of a double, then the threshold is specified for each dimension of each + * variable type. This parameter then maps from a character indicating the + * variable type to a Vector of thresholds for each dimension of that + * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, + * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate + * entries would be added with: + * \code + FastMap thresholds; + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); + // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = + Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold + params.relinearizeThreshold = thresholds; + \endcode + */ + RelinearizationThreshold relinearizeThreshold; + + int relinearizeSkip; ///< Only relinearize any variables every + ///< relinearizeSkip calls to ISAM2::update (default: + ///< 10) + + bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize + ///< any variables (default: true) + + bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error + ///< before and after the update, to return in + ///< ISAM2Result from update() + + enum Factorization { CHOLESKY, QR }; + /** Specifies whether to use QR or CHOESKY numerical factorization (default: + * CHOLESKY). Cholesky is faster but potentially numerically unstable for + * poorly-conditioned problems, which can occur when uncertainty is very low + * in some variables (or dimensions of variables) and very high in others. QR + * is slower but more numerically stable in poorly-conditioned problems. We + * suggest using the default of Cholesky unless gtsam sometimes throws + * IndefiniteLinearSystemException when your problem's Hessian is actually + * positive definite. For positive definite problems, numerical error + * accumulation can cause the problem to become numerically negative or + * indefinite as solving proceeds, especially when using Cholesky. + */ + Factorization factorization; + + /** Whether to cache linear factors (default: true). + * This can improve performance if linearization is expensive, but can hurt + * performance if linearization is very cleap due to computation to look up + * additional keys. + */ + bool cacheLinearizedFactors; + + KeyFormatter + keyFormatter; ///< A KeyFormatter for when keys are printed during + ///< debugging (default: DefaultKeyFormatter) + + bool enableDetailedResults; ///< Whether to compute and return + ///< ISAM2Result::detailedResults, this can + ///< increase running time (default: false) + + /** Check variables for relinearization in tree-order, stopping the check once + * a variable does not need to be relinearized (default: false). This can + * improve speed by only checking a small part of the top of the tree. + * However, variables below the check cut-off can accumulate significant + * deltas without triggering relinearization. This is particularly useful in + * exploration scenarios where real-time performance is desired over + * correctness. Use with caution. + */ + bool enablePartialRelinearizationCheck; + + /// 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 + /// cost of having to search for slots every time a factor is added. + bool findUnusedFactorSlots; + + /** + * Specify parameters as constructor arguments + * See the documentation of member variables above. + */ + ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), + RelinearizationThreshold _relinearizeThreshold = 0.1, + int _relinearizeSkip = 10, bool _enableRelinearization = true, + bool _evaluateNonlinearError = false, + Factorization _factorization = ISAM2Params::CHOLESKY, + bool _cacheLinearizedFactors = true, + const KeyFormatter& _keyFormatter = + DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter + ) + : optimizationParams(_optimizationParams), + relinearizeThreshold(_relinearizeThreshold), + relinearizeSkip(_relinearizeSkip), + enableRelinearization(_enableRelinearization), + evaluateNonlinearError(_evaluateNonlinearError), + factorization(_factorization), + cacheLinearizedFactors(_cacheLinearizedFactors), + keyFormatter(_keyFormatter), + enableDetailedResults(false), + enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} + + /// print iSAM2 parameters + void print(const std::string& str = "") const { + using std::cout; + cout << str << "\n"; + + static const std::string kStr("optimizationParams: "); + if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + boost::get(optimizationParams).print(); + else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) + boost::get(optimizationParams).print(kStr); + else + cout << kStr << "{unknown type}\n"; + + cout << "relinearizeThreshold: "; + if (relinearizeThreshold.type() == typeid(double)) { + cout << boost::get(relinearizeThreshold) << "\n"; + } else { + cout << "{mapped}\n"; + for (const ISAM2ThresholdMapValue& value : + boost::get(relinearizeThreshold)) { + cout << " '" << value.first + << "' -> [" << value.second.transpose() << " ]\n"; + } + } + + cout << "relinearizeSkip: " << relinearizeSkip << "\n"; + cout << "enableRelinearization: " << enableRelinearization + << "\n"; + cout << "evaluateNonlinearError: " << evaluateNonlinearError + << "\n"; + cout << "factorization: " + << factorizationTranslator(factorization) << "\n"; + cout << "cacheLinearizedFactors: " << cacheLinearizedFactors + << "\n"; + cout << "enableDetailedResults: " << enableDetailedResults + << "\n"; + cout << "enablePartialRelinearizationCheck: " + << enablePartialRelinearizationCheck << "\n"; + cout << "findUnusedFactorSlots: " << findUnusedFactorSlots + << "\n"; + cout.flush(); + } + + /// @name Getters and Setters for all properties + /// @{ + + OptimizationParams getOptimizationParams() const { + return this->optimizationParams; + } + RelinearizationThreshold getRelinearizeThreshold() const { + return relinearizeThreshold; + } + int getRelinearizeSkip() const { return relinearizeSkip; } + bool isEnableRelinearization() const { return enableRelinearization; } + bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } + std::string getFactorization() const { + return factorizationTranslator(factorization); + } + bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } + KeyFormatter getKeyFormatter() const { return keyFormatter; } + bool isEnableDetailedResults() const { return enableDetailedResults; } + bool isEnablePartialRelinearizationCheck() const { + return enablePartialRelinearizationCheck; + } + + void setOptimizationParams(OptimizationParams optimizationParams) { + this->optimizationParams = optimizationParams; + } + void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { + this->relinearizeThreshold = relinearizeThreshold; + } + void setRelinearizeSkip(int relinearizeSkip) { + this->relinearizeSkip = relinearizeSkip; + } + void setEnableRelinearization(bool enableRelinearization) { + this->enableRelinearization = enableRelinearization; + } + void setEvaluateNonlinearError(bool evaluateNonlinearError) { + this->evaluateNonlinearError = evaluateNonlinearError; + } + void setFactorization(const std::string& factorization) { + this->factorization = factorizationTranslator(factorization); + } + void setCacheLinearizedFactors(bool cacheLinearizedFactors) { + this->cacheLinearizedFactors = cacheLinearizedFactors; + } + void setKeyFormatter(KeyFormatter keyFormatter) { + this->keyFormatter = keyFormatter; + } + void setEnableDetailedResults(bool enableDetailedResults) { + this->enableDetailedResults = enableDetailedResults; + } + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck) { + this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return factorization == CHOLESKY + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; + } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h new file mode 100644 index 000000000..1539d90c4 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Result.h @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Result.h + * @brief Class that stores detailed iSAM2 result. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include + +namespace gtsam { + +typedef FastVector FactorIndices; + +/** + * @addtogroup ISAM2 + * This struct is returned from ISAM2::update() and contains information about + * the update that is useful for determining whether the solution is + * converging, and about how much work was required for the update. See member + * variables for details and information about each entry. + */ +struct GTSAM_EXPORT ISAM2Result { + /** The nonlinear error of all of the factors, \a including new factors and + * variables added during the current call to ISAM2::update(). This error is + * calculated using the following variable values: + * \li Pre-existing variables will be evaluated by combining their + * linearization point before this call to update, with their partial linear + * delta, as computed by ISAM2::calculateEstimate(). + * \li New variables will be evaluated at their initialization points passed + * into the current call to update. + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorBefore; + + /** The nonlinear error of all of the factors computed after the current + * update, meaning that variables above the relinearization threshold + * (ISAM2Params::relinearizeThreshold) have been relinearized and new + * variables have undergone one linear update. Variable values are + * again computed by combining their linearization points with their + * partial linear deltas, by ISAM2::calculateEstimate(). + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorAfter; + + /** The number of variables that were relinearized because their linear + * deltas exceeded the reslinearization threshold + * (ISAM2Params::relinearizeThreshold), combined with any additional + * variables that had to be relinearized because they were involved in + * the same factor as a variable above the relinearization threshold. + * On steps where no relinearization is considered + * (see ISAM2Params::relinearizeSkip), this count will be zero. + */ + size_t variablesRelinearized; + + /** The number of variables that were reeliminated as parts of the Bayes' + * Tree were recalculated, due to new factors. When loop closures occur, + * this count will be large as the new loop-closing factors will tend to + * involve variables far away from the root, and everything up to the root + * will be reeliminated. + */ + size_t variablesReeliminated; + + /** The number of factors that were included in reelimination of the Bayes' + * tree. */ + size_t factorsRecalculated; + + /** The number of cliques in the Bayes' Tree */ + size_t cliques; + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors to ISAM2::update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + FactorIndices newFactorsIndices; + + /** A struct holding detailed results, which must be enabled with + * ISAM2Params::enableDetailedResults. + */ + struct DetailedResults { + /** The status of a single variable, this struct is stored in + * DetailedResults::variableStatus */ + struct VariableStatus { + /** Whether the variable was just reeliminated, due to being relinearized, + * observed, new, or on the path up to the root clique from another + * reeliminated variable. */ + bool isReeliminated; + bool isAboveRelinThreshold; ///< Whether the variable was just + ///< relinearized due to being above the + ///< relinearization threshold + bool isRelinearizeInvolved; ///< Whether the variable was below the + ///< relinearization threshold but was + ///< relinearized by being involved in a + ///< factor with a variable above the + ///< relinearization threshold + bool isRelinearized; /// Whether the variable was relinearized, either by + /// being above the relinearization threshold or by + /// involvement. + bool isObserved; ///< Whether the variable was just involved in new + ///< factors + bool isNew; ///< Whether the variable itself was just added + bool inRootClique; ///< Whether the variable is in the root clique + VariableStatus() + : isReeliminated(false), + isAboveRelinThreshold(false), + isRelinearizeInvolved(false), + isRelinearized(false), + isObserved(false), + isNew(false), + inRootClique(false) {} + }; + + /** The status of each variable during this update, see VariableStatus. + */ + FastMap variableStatus; + }; + + /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See + * Detail for information about the results data stored here. */ + boost::optional detail; + + void print(const std::string str = "") const { + using std::cout; + cout << str << " Reelimintated: " << variablesReeliminated + << " Relinearized: " << variablesRelinearized + << " Cliques: " << cliques << std::endl; + } + + /** Getters and Setters */ + size_t getVariablesRelinearized() const { return variablesRelinearized; } + size_t getVariablesReeliminated() const { return variablesReeliminated; } + size_t getCliques() const { return cliques; } +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 904e08770..f1135d2f0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -23,6 +23,7 @@ #include #include #include +#include class NonlinearOptimizerMoreOptimizationTest; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 85f694bd2..35b0770c2 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -48,6 +48,9 @@ protected: public: + /// Default constructor only for Cython wrapper + Marginals(){} + /** Construct a marginals class. * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). @@ -91,6 +94,9 @@ protected: FastMap indices_; public: + /// Default constructor only for Cython wrapper + JointMarginal() {} + /** Access a block, corresponding to a pair of variables, of the joint * marginal. Each block is accessed by its "vertical position", * corresponding to the variable with nonlinear Key \c iVariable and diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ed811764a..a147f505e 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { namespace internal { diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index 19a390c45..bd5465dda 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -113,9 +113,9 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { // Small cache of A|b|model indexed by dimension. Can save many mallocs. mutable std::vector noiseModelCache; CachedModel* getCachedModel(size_t dim) const { - if (dim > noiseModelCache.size()) - noiseModelCache.resize(dim); - CachedModel* item = &noiseModelCache[dim - 1]; + if (dim >= noiseModelCache.size()) + noiseModelCache.resize(dim+1); + CachedModel* item = &noiseModelCache[dim]; if (!item->model) *item = CachedModel(dim, 1.0 / std::sqrt(lambda)); return item; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index aa50ce73f..a85118c00 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -231,14 +231,22 @@ TEST(AdaptAutoDiff, AdaptAutoDiff) { /* ************************************************************************* */ // Test AutoDiff wrapper in an expression TEST(AdaptAutoDiff, SnavelyExpression) { + typedef AdaptAutoDiff Adaptor; + Expression P(1); Expression X(2); - typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); + + std::size_t RecordSize = + sizeof(internal::BinaryExpression::Record); + EXPECT_LONGS_EQUAL( - sizeof(internal::BinaryExpression::Record), - expression.traceSize()); + internal::upAligned(RecordSize) + P.traceSize() + X.traceSize(), + expression.traceSize()); + set expected = list_of(1)(2); + EXPECT(expected == expression.keys()); } diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 208f0b284..5fc4e208d 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -23,6 +23,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/matlab.h b/gtsam/nonlinear/utilities.h similarity index 88% rename from matlab.h rename to gtsam/nonlinear/utilities.h index 5e144730d..3816f26f8 100644 --- a/matlab.h +++ b/gtsam/nonlinear/utilities.h @@ -17,7 +17,9 @@ #pragma once +#include #include +#include #include #include #include @@ -43,7 +45,7 @@ FastList createKeyList(const Vector& I) { } // Create a KeyList from indices using symbol -FastList createKeyList(string s, const Vector& I) { +FastList createKeyList(std::string s, const Vector& I) { FastList set; char c = s[0]; for (int i = 0; i < I.size(); i++) @@ -60,7 +62,7 @@ FastVector createKeyVector(const Vector& I) { } // Create a KeyVector from indices using symbol -FastVector createKeyVector(string s, const Vector& I) { +FastVector createKeyVector(std::string s, const Vector& I) { FastVector set; char c = s[0]; for (int i = 0; i < I.size(); i++) @@ -77,7 +79,7 @@ KeySet createKeySet(const Vector& I) { } // Create a KeySet from indices using symbol -KeySet createKeySet(string s, const Vector& I) { +KeySet createKeySet(std::string s, const Vector& I) { KeySet set; char c = s[0]; for (int i = 0; i < I.size(); i++) @@ -248,6 +250,32 @@ Values localToWorld(const Values& local, const Pose2& base, return world; } -} +} // namespace utilities + +/** + * For Python __str__(). + * Redirect std cout to a string stream so we can return a string representation + * of an object when it prints to cout. + * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string + */ +struct RedirectCout { + /// constructor -- redirect stdout buffer to a stringstream buffer + RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} + + /// return the string + std::string str() const { + return ssBuffer_.str(); + } + + /// destructor -- redirect stdout buffer to its original buffer + ~RedirectCout() { + std::cout.rdbuf(coutBuffer_); + } + +private: + std::stringstream ssBuffer_; + std::streambuf* coutBuffer_; +}; + } diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 73ff34d2a..c7309786d 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -38,8 +38,9 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; -Key poseKey(1); -Key pointKey(2); +// Keys are deliberately *not* in sorted order to test that case. +Key poseKey(2); +Key pointKey(1); double measurement(10.0); /* ************************************************************************* */ @@ -101,8 +102,13 @@ TEST( RangeFactor, ConstructorWithTransform) { RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); + KeyVector expected; + expected.push_back(2); + expected.push_back(1); + CHECK(factor2D.keys() == expected); RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); + CHECK(factor3D.keys() == expected); } /* ************************************************************************* */ @@ -395,4 +401,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index bfd7d4e34..f3fd49fa7 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -92,10 +92,10 @@ namespace gtsam { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR - typename traits::ChartJacobian::Fixed Hlocal; + typename traits::ChartJacobian::Jacobian Hlocal; Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); - if (H1) *H2 = Hlocal * (*H2); + if (H2) *H2 = Hlocal * (*H2); return rval; #else return traits::Local(measured_, hx); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e6b312b95..2e921bfef 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -80,6 +80,8 @@ public: return error; } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -198,6 +200,8 @@ public: return f_ * reprojectionError; } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -281,6 +285,8 @@ public: } } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f049e9d62..58408e7e3 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -120,9 +120,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { boost::shared_ptr > pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.add( - BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->noiseModel())); + pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } @@ -330,7 +329,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); initialPose.insert(keyAnchor, Pose3()); - pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); + pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index d09627b77..3f5290c58 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -51,7 +51,7 @@ public: /// Constructor JacobianFactorQ(const FastVector& keys, - const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const std::vector >& FBlocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 77102c24b..9e61f5324 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -29,7 +29,7 @@ public: * Constructor */ JacobianFactorQR(const FastVector& keys, - const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const std::vector >& FBlocks, const Matrix& E, const Matrix3& P, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : Base() { diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 8afe0bcbf..e7bc5864d 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -59,7 +59,7 @@ public: * @Fblocks: */ JacobianFactorSVD(const FastVector& keys, - const std::vector& Fblocks, const Matrix& Enull, + const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : Base() { diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 7c481d0c8..47c9a4c7b 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -36,7 +36,7 @@ protected: typedef Eigen::Matrix MatrixZD; ///< type of an F block typedef Eigen::Matrix MatrixDD; ///< camera hessian - const std::vector FBlocks_; ///< All ZDim*D F blocks (one for each camera) + const std::vector > FBlocks_; ///< All ZDim*D F blocks (one for each camera) const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Vector b_; ///< 2m-dimensional RHS vector @@ -49,7 +49,7 @@ public: /// Construct from blocks of F, E, inv(E'*E), and RHS vector b RegularImplicitSchurFactor(const FastVector& keys, - const std::vector& FBlocks, const Matrix& E, const Matrix& P, + const std::vector >& FBlocks, const Matrix& E, const Matrix& P, const Vector& b) : GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { } @@ -58,7 +58,7 @@ public: virtual ~RegularImplicitSchurFactor() { } - std::vector& FBlocks() const { + std::vector >& FBlocks() const { return FBlocks_; } @@ -252,7 +252,7 @@ public: y += F.transpose() * e3; } - typedef std::vector Error2s; + typedef std::vector> Error2s; /** * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 93d2ff218..497ebbc5b 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -50,12 +50,14 @@ private: typedef NonlinearFactor Base; typedef SmartFactorBase This; typedef typename CAMERA::Measurement Z; + typedef typename CAMERA::MeasurementVector ZVector; public: static const int Dim = traits::dimension; ///< Camera dimension static const int ZDim = traits::dimension; ///< Measurement dimension typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrpt camera) + typedef std::vector > FBlocks; // vector of F blocks protected: /** @@ -71,14 +73,14 @@ protected: * We keep a copy of measurements for I/O and computing the error. * The order is kept the same as the keys that we use to create the factor. */ - std::vector measured_; + ZVector measured_; /// @name Pose of the camera in the body frame boost::optional body_P_sensor_; ///< Pose of the camera in the body frame /// @} // Cache for Fblocks, to avoid a malloc ever time we re-linearize - mutable std::vector Fblocks; + mutable FBlocks Fs; public: @@ -97,7 +99,7 @@ public: SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, boost::optional body_P_sensor = boost::none, size_t expectedNumberCameras = 10) - : body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) { + : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) { if (!sharedNoiseModel) throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); @@ -129,7 +131,7 @@ public: /** * Add a bunch of measurements, together with the camera keys */ - void add(std::vector& measurements, std::vector& cameraKeys) { + void add(ZVector& measurements, std::vector& cameraKeys) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); this->keys_.push_back(cameraKeys.at(i)); @@ -154,7 +156,7 @@ public: } /** return the measurements */ - const std::vector& measured() const { + const ZVector& measured() const { return measured_; } @@ -202,7 +204,7 @@ public: boost::optional Fs = boost::none, // boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); - if(body_P_sensor_){ + if(body_P_sensor_ && Fs){ for(size_t i=0; i < Fs->size(); i++){ Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Matrix J(6, 6); @@ -210,9 +212,17 @@ public: Fs->at(i) = Fs->at(i) * J; } } + correctForMissingMeasurements(cameras, ue, Fs, E); return ue; } + /** + * This corrects the Jacobians for the case in which some pixel measurement is missing (nan) + * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, + boost::optional E = boost::none) const {} + /** * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] * Noise model applied @@ -250,22 +260,22 @@ public: * TODO: Kill this obsolete method */ template - void computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, + void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b, const Cameras& cameras, const POINT& point) const { // Project into Camera set and calculate derivatives // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| // = |A*dx - (z-h(x_bar))| - b = -unwhitenedError(cameras, point, Fblocks, E); + b = -unwhitenedError(cameras, point, Fs, E); } /// SVD version template - void computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull, Vector& b, const Cameras& cameras, const POINT& point) const { Matrix E; - computeJacobians(Fblocks, E, b, cameras, point); + computeJacobians(Fs, E, b, cameras, point); static const int N = FixedDimension::value; // 2 (Unit3) or 3 (Point3) @@ -283,10 +293,10 @@ public: Matrix E; Vector b; - computeJacobians(Fblocks, E, b, cameras, point); + computeJacobians(Fs, E, b, cameras, point); // build augmented hessian - SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b); + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b); return boost::make_shared >(keys_, augmentedHessian); @@ -303,12 +313,12 @@ public: const FastVector allKeys) const { Matrix E; Vector b; - computeJacobians(Fblocks, E, b, cameras, point); - Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian); + computeJacobians(Fs, E, b, cameras, point); + Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian); } /// Whiten the Jacobians computed by computeJacobians using noiseModel_ - void whitenJacobians(std::vector& F, Matrix& E, Vector& b) const { + void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const { noiseModel_->WhitenSystem(E, b); // TODO make WhitenInPlace work with any dense matrix type for (size_t i = 0; i < F.size(); i++) @@ -321,7 +331,7 @@ public: double lambda = 0.0, bool diagonalDamping = false) const { Matrix E; Vector b; - std::vector F; + FBlocks F; computeJacobians(F, E, b, cameras, point); whitenJacobians(F, E, b); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); @@ -337,7 +347,7 @@ public: bool diagonalDamping = false) const { Matrix E; Vector b; - std::vector F; + FBlocks F; computeJacobians(F, E, b, cameras, point); const size_t M = b.size(); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); @@ -352,7 +362,7 @@ public: boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t m = this->keys_.size(); - std::vector F; + FBlocks F; Vector b; const size_t M = ZDim * m; Matrix E0(M, M - 3); @@ -363,12 +373,12 @@ public: } /// Create BIG block-diagonal matrix F from Fblocks - static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { - size_t m = Fblocks.size(); + static void FillDiagonalF(const FBlocks& Fs, Matrix& F) { + size_t m = Fs.size(); F.resize(ZDim * m, Dim * m); F.setZero(); for (size_t i = 0; i < m; ++i) - F.block(ZDim * i, Dim * i) = Fblocks.at(i); + F.block(ZDim * i, Dim * i) = Fs.at(i); } diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h new file mode 100644 index 000000000..761703f8b --- /dev/null +++ b/gtsam/slam/SmartFactorParams.h @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartFactorParams + * @brief Collect common parameters for SmartProjection and SmartStereoProjection factors + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors + */ +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart (stereo) projection factors + */ +struct GTSAM_EXPORT SmartProjectionParams { + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + TriangulationParameters triangulation; + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false, double retriangulationTh = 1e-5) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str = "") const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + // get class variables + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + double getRetriangulationThreshold() const { + return retriangulationThreshold; + } + // set class variables + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRetriangulationThreshold(double retriangulationTh) { + retriangulationThreshold = retriangulationTh; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(linearizationMode); + ar & BOOST_SERIALIZATION_NVP(degeneracyMode); + ar & BOOST_SERIALIZATION_NVP(triangulation); + ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); + ar & BOOST_SERIALIZATION_NVP(throwCheirality); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality); + } +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index d34ba11e3..0d9f90d22 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -31,109 +32,6 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to -enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD -}; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY -}; - -/* - * Parameters for the smart projection factors - */ -struct GTSAM_EXPORT SmartProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - // Constructor - SmartProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false, double retriangulationTh = 1e-5) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - double getRetriangulationThreshold() const { - return retriangulationThreshold; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setRetriangulationThreshold(double retriangulationTh) { - retriangulationThreshold = retriangulationTh; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - -private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(linearizationMode); - ar & BOOST_SERIALIZATION_NVP(degeneracyMode); - ar & BOOST_SERIALIZATION_NVP(triangulation); - ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); - ar & BOOST_SERIALIZATION_NVP(throwCheirality); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality); - } -}; - /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. * This factor operates with monocular cameras, where a camera is expected to @@ -163,7 +61,7 @@ protected: /// @name Caching triangulation /// @{ mutable TriangulationResult result_; ///< result from triangulateSafe - mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + mutable std::vector > cameraPosesTriangulation_; ///< current triangulation poses /// @} public: @@ -305,7 +203,7 @@ public: } // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). - std::vector Fblocks; + std::vector > Fblocks; Matrix E; Vector b; computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); @@ -439,7 +337,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - std::vector& Fblocks, Matrix& E, Vector& b, + std::vector >& Fblocks, Matrix& E, Vector& b, const Cameras& cameras) const { if (!result_) { @@ -456,7 +354,7 @@ public: /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( - std::vector& Fblocks, Matrix& E, Vector& b, + std::vector >& Fblocks, Matrix& E, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); @@ -467,7 +365,7 @@ public: /// takes values bool triangulateAndComputeJacobiansSVD( - std::vector& Fblocks, Matrix& Enull, Vector& b, + std::vector >& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index dc25026d2..ba51864f1 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -11,7 +11,7 @@ /** * @file dataset.cpp * @date Jan 22, 2010 - * @author nikai, Luca Carlone + * @author Kai Ni, Luca Carlone, Frank Dellaert * @brief utility functions for loading datasets */ @@ -56,8 +56,10 @@ namespace gtsam { string findExampleDataFile(const string& name) { // Search source tree and installed location vector rootsToSearch; - rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt - rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt + + // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt + rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); + rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; @@ -75,12 +77,11 @@ string findExampleDataFile(const string& name) { } // If we did not return already, then we did not find the file - throw -invalid_argument( - "gtsam::findExampleDataFile could not find a matching file in\n" - GTSAM_SOURCE_TREE_DATASET_DIR " or\n" - GTSAM_INSTALLED_DATASET_DIR " named\n" + - name + ", " + name + ".graph, or " + name + ".txt"); + throw invalid_argument( + "gtsam::findExampleDataFile could not find a matching file in\n" + GTSAM_SOURCE_TREE_DATASET_DIR " or\n" + GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name + + ".graph, or " + name + ".txt"); } /* ************************************************************************* */ @@ -98,6 +99,7 @@ string createRewrittenFileName(const string& name) { return newpath.string(); } + /* ************************************************************************* */ #endif @@ -116,23 +118,20 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, double v1, v2, v3, v4, v5, v6; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; - if (noiseFormat == NoiseFormatAUTO) - { - // Try to guess covariance matrix layout - if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) - { - // NoiseFormatGRAPH - noiseFormat = NoiseFormatGRAPH; - } - else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) - { - // NoiseFormatCOV - noiseFormat = NoiseFormatCOV; - } - else - { - throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); - } + if (noiseFormat == NoiseFormatAUTO) { + // Try to guess covariance matrix layout + if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 == 0.0) { + // NoiseFormatGRAPH + noiseFormat = NoiseFormatGRAPH; + } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 + && v6 != 0.0) { + // NoiseFormatCOV + noiseFormat = NoiseFormatCOV; + } else { + throw std::invalid_argument( + "load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); + } } // Read matrix and check that diagonal entries are non-zero @@ -195,6 +194,32 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, } } +/* ************************************************************************* */ +boost::optional parseVertex(istream& is, const string& tag) { + if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { + Key id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + return IndexedPose(id, Pose2(x, y, yaw)); + } else { + return boost::none; + } +} + +/* ************************************************************************* */ +boost::optional parseEdge(istream& is, const string& tag) { + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") + || (tag == "ODOMETRY")) { + + Key id1, id2; + double x, y, yaw; + is >> id1 >> id2 >> x >> y >> yaw; + return IndexedEdge(pair(id1, id2), Pose2(x, y, yaw)); + } else { + return boost::none; + } +} + /* ************************************************************************* */ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, bool addNoise, bool smart, NoiseFormat noiseFormat, @@ -214,16 +239,15 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (!(is >> tag)) break; - if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - Key id; - double x, y, yaw; - is >> id >> x >> y >> yaw; + const auto indexed_pose = parseVertex(is, tag); + if (indexed_pose) { + Key id = indexed_pose->first; // optional filter if (maxID && id >= maxID) continue; - initial->insert(id, Pose2(x, y, yaw)); + initial->insert(id, indexed_pose->second); } is.ignore(LINESIZE, '\n'); } @@ -251,13 +275,10 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (!(is >> tag)) break; - if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") - || (tag == "ODOMETRY")) { - - // Read transform - double x, y, yaw; - is >> id1 >> id2 >> x >> y >> yaw; - Pose2 l1Xl2(x, y, yaw); + auto between_pose = parseEdge(is, tag); + if (between_pose) { + std::tie(id1, id2) = between_pose->first; + Pose2& l1Xl2 = between_pose->second; // read noise model SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc7804c2d..929ada2c1 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -33,6 +33,7 @@ #include #include // for pair #include +#include namespace gtsam { @@ -71,6 +72,26 @@ enum KernelFunctionType { KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY }; +/// Return type for auxiliary functions +typedef std::pair IndexedPose; +typedef std::pair, Pose2> IndexedEdge; + +/** + * Parse TORO/G2O vertex "id x y yaw" + * @param is input stream + * @param tag string parsed from input stream, will only parse if vertex type + */ +GTSAM_EXPORT boost::optional parseVertex(std::istream& is, + const std::string& tag); + +/** + * Parse TORO/G2O edge "id1 id2 x y yaw" + * @param is input stream + * @param tag string parsed from input stream, will only parse if edge type + */ +GTSAM_EXPORT boost::optional parseEdge(std::istream& is, + const std::string& tag); + /// Return type for load functions typedef std::pair GraphAndValues; diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index ccad83f01..4abc59305 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -132,7 +132,7 @@ CAMERA perturbCameraPose(const CAMERA& camera) { template void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, - const CAMERA& cam3, Point3 landmark, vector& measurements_cam) { + const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) { Point2 cam1_uv1 = cam1.project(landmark); Point2 cam2_uv1 = cam2.project(landmark); Point2 cam3_uv1 = cam3.project(landmark); diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 7a95e0fa9..e34e13279 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -90,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.push_back(PriorFactor(1, pose1, sigma)); - graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + graph.emplace_shared >(1, pose1, sigma); + graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1dd4e8abc..39c2d3722 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -26,6 +26,9 @@ #include +#include +#include + using namespace gtsam::symbol_shorthand; using namespace std; using namespace gtsam; @@ -39,6 +42,37 @@ TEST(dataSet, findExampleDataFile) { EXPECT(assert_equal(expected_end, actual_end)); } +/* ************************************************************************* */ +TEST( dataSet, parseVertex) +{ + const string str = "VERTEX2 1 2.000000 3.000000 4.000000"; + istringstream is(str); + string tag; + EXPECT(is >> tag); + const auto actual = parseVertex(is, tag); + EXPECT(actual); + if (actual) { + EXPECT_LONGS_EQUAL(1, actual->first); + EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); + } +} + +/* ************************************************************************* */ +TEST( dataSet, parseEdge) +{ + const string str = "EDGE2 0 1 2.000000 3.000000 4.000000"; + istringstream is(str); + string tag; + EXPECT(is >> tag); + const auto actual = parseEdge(is, tag); + EXPECT(actual); + if (actual) { + pair expected(0, 1); + EXPECT(expected == actual->first); + EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); + } +} + /* ************************************************************************* */ TEST( dataSet, load2D) { @@ -112,18 +146,18 @@ TEST( dataSet, readG2o) noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + 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)); } @@ -164,27 +198,27 @@ TEST( dataSet, readG2o3D) Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); + expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); + expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); + expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); + expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); + expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -224,7 +258,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); } @@ -242,18 +276,18 @@ TEST( dataSet, readG2oHuber) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + 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)); } @@ -270,18 +304,18 @@ TEST( dataSet, readG2oTukey) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + 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)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 2e3d613d6..d33551edf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -178,7 +178,7 @@ TEST (EssentialMatrixFactor, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); + graph.emplace_shared(1, pA(i), pB(i), model1); // Check error at ground truth Values truth; @@ -251,7 +251,7 @@ TEST (EssentialMatrixFactor2, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); + graph.emplace_shared(100, i, pA(i), pB(i), model2); // Check error at ground truth Values truth; @@ -323,7 +323,7 @@ TEST (EssentialMatrixFactor3, minimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2)); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); // Check error at ground truth Values truth; @@ -391,7 +391,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); + graph.emplace_shared(1, pA(i), pB(i), model1, K); // Check error at ground truth Values truth; @@ -465,7 +465,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); + graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b99cb5d9c..8e0b5ad8f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -368,9 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; @@ -385,12 +385,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - 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.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), GeneralCamera()); @@ -413,15 +413,15 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back( - PriorFactor(X(0), CalibratedCamera(), - noiseModel::Isotropic::Sigma(6, 1.))); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + PriorFactor >(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.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), CalibratedCamera()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9fda7d745..d8ac0aa5b 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -369,9 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index f955aa191..24b818835 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) { // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails NonlinearFactorGraph graph; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared(lB2, tA1, lA2); // hard constraints on points double error_gain = 1000.0; - graph.push_back(NonlinearEquality(lA1, local1, error_gain)); - graph.push_back(NonlinearEquality(lA2, local2, error_gain)); - graph.push_back(NonlinearEquality(lB1, global1, error_gain)); - graph.push_back(NonlinearEquality(lB2, global2, error_gain)); + graph.emplace_shared >(lA1, local1, error_gain); + graph.emplace_shared >(lA2, local2, error_gain); + graph.emplace_shared >(lB1, global1, error_gain); + graph.emplace_shared >(lB2, global2, error_gain); // create initial estimate Values init; @@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lB1, global, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lB1, global, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; @@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lA1, local, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lA1, local, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 4184d6769..6f9371e68 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 = list_of(F0)(F1)(F3); const FastVector keys = list_of(0)(1)(3); // RHS and sigmas const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 9bb296444..caf3d31df 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -89,9 +89,9 @@ TEST (RotateFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(3, 0.01); - graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); - graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); - graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); + graph.emplace_shared(1, i1Ri2, c1Zc2, model); + graph.emplace_shared(1, i2Ri3, c2Zc3, model); + graph.emplace_shared(1, i3Ri4, c3Zc4, model); // Check error at ground truth Values truth; @@ -162,9 +162,9 @@ TEST (RotateDirectionsFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(2, 0.01); - graph.add(RotateDirectionsFactor(1, p1, z1, model)); - graph.add(RotateDirectionsFactor(1, p2, z2, model)); - graph.add(RotateDirectionsFactor(1, p3, z3, model)); + graph.emplace_shared(1, p1, z1, model); + graph.emplace_shared(1, p2, z2, model); + graph.emplace_shared(1, p3, z3, model); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 0db8149eb..17fec7b9f 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -166,7 +166,7 @@ TEST( SmartProjectionCameraFactor, noisy ) { EXPECT(assert_equal(expected, actual, 1)); SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); - vector measurements; + Point2Vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); @@ -186,7 +186,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { using namespace vanilla; // Project three landmarks into three cameras - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); @@ -209,8 +209,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); // Create initial estimate Values initial; @@ -288,7 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { using namespace vanilla; // Project three landmarks into three cameras - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); @@ -321,8 +321,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -360,7 +360,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { using namespace vanilla; - vector measurements_cam1, measurements_cam2, measurements_cam3, + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4, measurements_cam5; // 1. Project three landmarks into three cameras and triangulate @@ -398,8 +398,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -440,7 +440,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { using namespace bundler; - vector measurements_cam1, measurements_cam2, measurements_cam3, + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4, measurements_cam5; // 1. Project three landmarks into three cameras and triangulate @@ -476,8 +476,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -516,7 +516,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { using namespace bundler; - vector measurements_cam1, measurements_cam2, measurements_cam3, + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4, measurements_cam5; // 1. Project three landmarks into three cameras and triangulate @@ -552,8 +552,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -768,8 +768,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { Point3 point(0,0,0); if (factor1->point()) point = *(factor1->point()); - vector Fblocks; - factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); + SmartFactor::FBlocks Fs; + factor1->computeJacobians(Fs, expectedE, expectedb, cameras, point); NonlinearFactorGraph generalGraph; SFMFactor sfm1(level_uv, unit2, c1, l1); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f0a864fdd..7d3d9c63c 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -155,8 +155,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) { // Calculate using computeJacobians Vector b; - vector Fblocks; - factor.computeJacobians(Fblocks, E, b, cameras, *point); + SmartFactor::FBlocks Fs; + 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); @@ -185,7 +185,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { double actualError1 = factor->error(values); SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); - vector measurements; + Point2Vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); @@ -228,7 +228,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(5, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ 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.emplace_shared >(x1, bodyPose1, noisePrior); + graph.emplace_shared >(x2, bodyPose2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -292,7 +292,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -319,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values groundTruth; groundTruth.insert(x1, cam1.pose()); @@ -363,7 +363,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); - vector measurements_cam1; + Point2Vector measurements_cam1; // Project 2 landmarks into 2 cameras measurements_cam1.push_back(cam1.project(landmark1)); @@ -454,7 +454,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { E(2, 0) = 10; E(2, 2) = 1; E(3, 1) = 10; - vector Fblocks = list_of(F1)(F2); + SmartFactor::FBlocks Fs = list_of(F1)(F2); Vector b(4); b.setZero(); @@ -466,7 +466,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { // createJacobianQFactor SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); Matrix3 P = (E.transpose() * E).inverse(); - JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n); + JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); boost::shared_ptr > actualQ = @@ -480,11 +480,11 @@ TEST( SmartProjectionPoseFactor, Factors ) { // Whiten for RegularImplicitSchurFactor (does not have noise model) model->WhitenSystem(E, b); Matrix3 whiteP = (E.transpose() * E).inverse(); - Fblocks[0] = model->Whiten(Fblocks[0]); - Fblocks[1] = model->Whiten(Fblocks[1]); + Fs[0] = model->Whiten(Fs[0]); + Fs[1] = model->Whiten(Fs[1]); // createRegularImplicitSchurFactor - RegularImplicitSchurFactor expected(keys, Fblocks, E, whiteP, b); + RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); boost::shared_ptr > actual = smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); @@ -525,7 +525,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -547,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -582,7 +582,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { views.push_back(x2); views.push_back(x3); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -614,8 +614,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -643,7 +643,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { views.push_back(x2); views.push_back(x3); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -675,8 +675,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -709,7 +709,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { // add fourth landmark Point3 landmark4(5, -0.5, 1); - vector measurements_cam1, measurements_cam2, measurements_cam3, + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; // Project 4 landmarks into three cameras @@ -747,8 +747,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values values; values.insert(x1, cam1.pose()); @@ -772,7 +772,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { views.push_back(x2); views.push_back(x3); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -800,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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 @@ -830,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { NonlinearFactorGraph graph; // Project three landmarks into three cameras - graph.push_back( - ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.emplace_shared >(x1, level_pose, noisePrior); + graph.emplace_shared >(x2, pose_right, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -892,7 +883,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -975,7 +966,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac Camera cam2(pose2, sharedK2); Camera cam3(pose3, sharedK2); - vector measurements_cam1, measurements_cam2; + Point2Vector measurements_cam1, measurements_cam2; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -1000,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -1022,6 +1011,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { + // this test considers a condition in which the cheirality constraint is triggered using namespace vanillaPose; vector views; @@ -1036,7 +1026,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -1068,11 +1058,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // 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), @@ -1096,8 +1084,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION EXPECT(assert_equal(Pose3(values.at(x3).rotation(), Point3(0,0,1)), result.at(x3))); +#else + // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation + // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) + EXPECT(assert_equal(pose3, result.at(x3),1e-3)); +#endif } /* *************************************************************************/ @@ -1112,7 +1106,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); - vector measurements_cam1; + Point2Vector measurements_cam1; measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); @@ -1144,7 +1138,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { views.push_back(x2); views.push_back(x3); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -1201,7 +1195,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { Camera cam2(level_pose, sharedK2); Camera cam3(level_pose, sharedK2); - vector measurements_cam1; + Point2Vector measurements_cam1; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); @@ -1259,7 +1253,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -1286,8 +1280,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -1330,7 +1324,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { // landmark3 at 3 meters now Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -1362,11 +1356,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // 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), @@ -1430,7 +1422,7 @@ TEST(SmartProjectionPoseFactor, serialize2) { // insert some measurments vector key_view; - vector meas_view; + Point2Vector meas_view; key_view.push_back(Symbol('x', 1)); meas_view.push_back(Point2(10, 10)); factor.add(meas_view, key_view); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 45b978c27..c802603db 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; - graph.push_back(NonlinearEquality(X(1), camera1)); + graph.emplace_shared >(X(1), camera1); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.push_back(GenericStereoFactor(measurement, model, X(1), L(1), K)); + graph.emplace_shared >(measurement, model, X(1), L(1), K); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 781b08d57..a1a8e3a90 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -20,3 +20,7 @@ list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") if("@GTSAM_USE_EIGEN_MKL@") list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") endif() + +if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") + list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") +endif() diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 6abfe4336..448e8b00e 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -118,6 +118,7 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}") endif(GTSAM_INSTALL_MATLAB_TOOLBOX) + # Build examples add_subdirectory(examples) diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 28f7e8444..fe1c83ce3 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -61,7 +61,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.push_back(PriorFactor(Symbol('K', 0), *noisy_K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -77,7 +77,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.push_back(PriorFactor(Symbol('x', pose_id), Pose3(m), poseNoise)); + graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys size_t x, l; @@ -89,9 +89,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //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.push_back( GenericStereoFactor(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K)); +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); - graph.push_back(GeneralSFMFactor2(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0))); + graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it @@ -107,7 +107,7 @@ int main(int argc, char** argv){ //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.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; LevenbergMarquardtParams params; diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c5ba616ed..94a70470a 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv){ //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.push_back(NonlinearEquality(1,firstPose)); + graph.emplace_shared >(1,firstPose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 40ad0cb52..c8023b23c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv){ //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.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index dda2c32e6..c503514a6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -18,7 +18,21 @@ */ #include +#include namespace gtsam { +/* ************************************************************************* */ +void Event::print(const std::string& s) const { + 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); +} + +/* ************************************************************************* */ + } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 40c70696b..d9bacd106 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,6 +21,7 @@ #include #include +#include namespace gtsam { @@ -59,15 +60,10 @@ public: } /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); - } + void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_ - other.time_) < tol - && traits::Equals(location_, other.location_, tol); - } + bool equals(const Event& other, double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index ba1445b20..00bd28ec7 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -126,7 +126,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( double side_len, double mean_side_len, double sigma_side_len, double min_vertex_dist, double min_side_len, const vector& existing_polys) { // get the current set of landmarks - std::vector lms; + Point2Vector lms; double d2 = side_len/2.0; lms.push_back(Point2( d2, d2)); lms.push_back(Point2(-d2, d2)); @@ -181,7 +181,7 @@ SimPolygon2D SimPolygon2D::randomRectangle( double side_len, double mean_side_len, double sigma_side_len, double min_vertex_dist, double min_side_len, const vector& existing_polys) { // get the current set of landmarks - std::vector lms; + Point2Vector lms; double d2 = side_len/2.0; lms.push_back(Point2( d2, d2)); lms.push_back(Point2(-d2, d2)); @@ -265,7 +265,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, /* ***************************************************************** */ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, - const std::vector& landmarks, double min_landmark_dist) { + const Point2Vector& landmarks, double min_landmark_dist) { for (size_t i=0; i& landmarks, + const Point2Vector& landmarks, const vector& obstacles, double min_landmark_dist) { for (size_t i=0; i& landmarks, + const Point2Vector& landmarks, const std::vector& obstacles, double min_landmark_dist) { boost::uniform_real<> gen_x(0.0, UR_corner.x() - LL_corner.x()); @@ -317,7 +317,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) { } /* ***************************************************************** */ -bool SimPolygon2D::nearExisting(const std::vector& S, +bool SimPolygon2D::nearExisting(const Point2Vector& S, const Point2& p, double threshold) { for(const Point2& Sp: S) if (distance2(Sp, p) < threshold) diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h index 835bb4083..0a0e0da7f 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.h +++ b/gtsam_unstable/geometry/SimPolygon2D.h @@ -18,7 +18,7 @@ namespace gtsam { */ class GTSAM_UNSTABLE_EXPORT SimPolygon2D { protected: - std::vector landmarks_; + Point2Vector landmarks_; static boost::minstd_rand rng; public: @@ -57,7 +57,7 @@ public: // access to underlying points const Point2& landmark(size_t i) const { return landmarks_[i]; } size_t size() const { return landmarks_.size(); } - const std::vector& vertices() const { return landmarks_; } + const Point2Vector& vertices() const { return landmarks_; } // testable requirements bool equals(const SimPolygon2D& p, double tol=1e-5) const; @@ -91,7 +91,7 @@ public: static bool insideBox(double s, const Point2& p); /** returns true iff p is within threshold of any point in S */ - static bool nearExisting(const std::vector& S, + static bool nearExisting(const Point2Vector& S, const Point2& p, double threshold); /** pick a random point uniformly over a box of side s */ @@ -105,11 +105,11 @@ public: /** pick a random point within a box that is further than dist d away from existing landmarks */ static Point2 randomBoundedPoint2(double boundary_size, - const std::vector& landmarks, double min_landmark_dist); + const Point2Vector& landmarks, double min_landmark_dist); /** pick a random point within a box that meets above requirements, as well as staying out of obstacles */ static Point2 randomBoundedPoint2(double boundary_size, - const std::vector& landmarks, + const Point2Vector& landmarks, const std::vector& obstacles, double min_landmark_dist); /** pick a random point that only avoid obstacles */ @@ -119,7 +119,7 @@ public: /** pick a random point in box defined by lower left and upper right corners */ static Point2 randomBoundedPoint2( const Point2& LL_corner, const Point2& UR_corner, - const std::vector& landmarks, + const Point2Vector& landmarks, const std::vector& obstacles, double min_landmark_dist); /** pick a random pose in a bounded area that is not in an obstacle */ diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 99b33182f..39c910826 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -103,7 +103,7 @@ class PoseRTV { #include class Pose3Upright { Pose3Upright(); - Pose3Upright(const gtsam::Pose3Upright& x); + Pose3Upright(const gtsam::Pose3Upright& other); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); Pose3Upright(double x, double y, double z, double theta); Pose3Upright(const gtsam::Pose2& pose, double z); @@ -141,7 +141,7 @@ class Pose3Upright { #include class BearingS2 { BearingS2(); - BearingS2(double azimuth, double elevation); + BearingS2(double azimuth_double, double elevation_double); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); gtsam::Rot2 azimuth() const; @@ -276,7 +276,7 @@ class SimPolygon2D { // Nonlinear factors from gtsam, for our Value types #include template -virtual class PriorFactor : gtsam::NonlinearFactor { +virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); void serializable() const; // enabling serialization functionality @@ -285,7 +285,7 @@ virtual class PriorFactor : gtsam::NonlinearFactor { #include template -virtual class BetweenFactor : gtsam::NonlinearFactor { +virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); void serializable() const; // enabling serialization functionality @@ -363,13 +363,13 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { void addRange(size_t key, double measuredRange); gtsam::Point2 triangulate(const gtsam::Values& x) const; - void print(string s) const; + //void print(string s) const; }; #include template -virtual class RangeFactor : gtsam::NonlinearFactor { +virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); void serializable() const; // enabling serialization functionality @@ -380,7 +380,7 @@ typedef gtsam::RangeFactor RangeFactorRTV; #include template -virtual class NonlinearEquality : gtsam::NonlinearFactor { +virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation @@ -391,7 +391,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { #include template -virtual class IMUFactor : gtsam::NonlinearFactor { +virtual class IMUFactor : gtsam::NoiseModelFactor { /** Standard constructor */ IMUFactor(Vector accel, Vector gyro, double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); @@ -409,7 +409,7 @@ virtual class IMUFactor : gtsam::NonlinearFactor { #include template -virtual class FullIMUFactor : gtsam::NonlinearFactor { +virtual class FullIMUFactor : gtsam::NoiseModelFactor { /** Standard constructor */ FullIMUFactor(Vector accel, Vector gyro, double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); @@ -425,13 +425,11 @@ virtual class FullIMUFactor : gtsam::NonlinearFactor { size_t key2() const; }; - #include virtual class DHeightPrior : gtsam::NonlinearFactor { DHeightPrior(size_t key, double height, const gtsam::noiseModel::Base* model); }; - virtual class DRollPrior : gtsam::NonlinearFactor { /** allows for explicit roll parameterization - uses canonical coordinate */ DRollPrior(size_t key, double wx, const gtsam::noiseModel::Base* model); @@ -439,12 +437,10 @@ virtual class DRollPrior : gtsam::NonlinearFactor { DRollPrior(size_t key, const gtsam::noiseModel::Base* model); }; - virtual class VelocityPrior : gtsam::NonlinearFactor { VelocityPrior(size_t key, Vector vel, const gtsam::noiseModel::Base* model); }; - virtual class DGroundConstraint : gtsam::NonlinearFactor { // Primary constructor allows for variable height of the "floor" DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model); @@ -470,6 +466,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor { Vector evaluateError(const gtsam::LieScalar& qk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& v) const; }; +#include virtual class PendulumFactor2 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g); @@ -492,18 +489,17 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { }; #include - -virtual class Reconstruction : gtsam::NonlinearFactor { +virtual class Reconstruction : gtsam::NoiseModelFactor { Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); - Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const; + Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, Vector xiK) const; }; -virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { +virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, double h, Matrix Inertia, Vector Fu, double m); - Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const; + Vector evaluateError(Vector xiK, Vector xiK_1, const gtsam::Pose3& gK) const; }; //************************************************************************* @@ -640,13 +636,13 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { // slam //************************************************************************* #include -virtual class RelativeElevationFactor: gtsam::NonlinearFactor { +virtual class RelativeElevationFactor: gtsam::NoiseModelFactor { RelativeElevationFactor(); RelativeElevationFactor(size_t poseKey, size_t pointKey, double measured, const gtsam::noiseModel::Base* model); double measured() const; - void print(string s) const; + //void print(string s) const; }; #include @@ -655,20 +651,20 @@ virtual class DummyFactor : gtsam::NonlinearFactor { }; #include -virtual class InvDepthFactorVariant1 : gtsam::NonlinearFactor { +virtual class InvDepthFactorVariant1 : gtsam::NoiseModelFactor { InvDepthFactorVariant1(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; #include -virtual class InvDepthFactorVariant2 : gtsam::NonlinearFactor { +virtual class InvDepthFactorVariant2 : gtsam::NoiseModelFactor { InvDepthFactorVariant2(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::Point3& referencePoint, const gtsam::noiseModel::Base* model); }; #include -virtual class InvDepthFactorVariant3a : gtsam::NonlinearFactor { +virtual class InvDepthFactorVariant3a : gtsam::NoiseModelFactor { InvDepthFactorVariant3a(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; -virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor { +virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor { InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; @@ -685,7 +681,7 @@ class Mechanization_bRn2 { static gtsam::Mechanization_bRn2 initialize(Matrix U, Matrix F, double g_e); gtsam::Mechanization_bRn2 correct(Vector dx) const; gtsam::Mechanization_bRn2 integrate(Vector u, double dt) const; - void print(string s) const; + //void print(string s) const; }; #include @@ -695,19 +691,17 @@ class AHRS { pair integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector u, double dt); pair aid(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, bool Farrel); pair aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, Vector f_expected, const gtsam::Rot3& increment); - void print(string s) const; + //void print(string s) const; }; // Tectonic SAM Factors #include -#include - //typedef gtsam::NoiseModelFactor2 NLPosePose; virtual class DeltaFactor : gtsam::NoiseModelFactor { DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); - void print(string s) const; + //void print(string s) const; }; //typedef gtsam::NoiseModelFactor4 diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 91cee3941..153aa7fda 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -51,7 +51,7 @@ struct LPPolicy { ++it) { size_t dim = lp.cost.getDim(it); Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g - graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b)); + graph.emplace_shared(*it, Matrix::Identity(dim, dim), b); } KeySet allKeys = lp.inequalities.keys(); @@ -67,8 +67,7 @@ struct LPPolicy { std::inserter(difference, difference.end())); for (Key k : difference) { size_t dim = lp.constrainedKeyDimMap().at(k); - graph.push_back( - JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); + graph.emplace_shared(k, Matrix::Identity(dim, dim), xk.at(k)); } } return graph; @@ -77,4 +76,4 @@ struct LPPolicy { using LPSolver = ActiveSetSolver; -} \ No newline at end of file +} diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 8bf6be56b..a105a39f0 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -189,12 +189,12 @@ TEST(LPSolver, overConstrainedLinearSystem) { TEST(LPSolver, overConstrainedLinearSystem2) { GaussianFactorGraph graph; - graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne, - noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne, - noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, - noiseModel::Constrained::All(1))); + graph.emplace_shared(1, I_1x1, 2, I_1x1, kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, + noiseModel::Constrained::All(1)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 74ee23fe5..4aff1b465 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); Values partialValues; partialValues.insert(1, optimalValues.at(1)); @@ -437,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: result) { - expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); + expectedGraph.emplace_shared(factor, partialValues); } @@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1181,11 +1181,11 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); @@ -1238,10 +1238,10 @@ 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.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1258,11 +1258,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(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)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1291,11 +1291,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index dc316e2ac..a0b6e2c1b 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -670,11 +670,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); @@ -724,10 +724,10 @@ 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.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -744,11 +744,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(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)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -774,11 +774,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 0c922fb9e..d6b7ab851 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -423,9 +423,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); @@ -441,7 +441,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, @@ -507,9 +507,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues); @@ -525,7 +525,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, @@ -1231,13 +1231,13 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1289,11 +1289,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); @@ -1350,10 +1350,10 @@ 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.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1406,11 +1406,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 65f74dc3c..fdf9f08b5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -639,13 +639,13 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 9391814d4..2240034af 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -27,22 +27,22 @@ 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.add(PriorFactor(x1, prior, priorNoise)); + graph.emplace_shared >(x1, prior, priorNoise); // Two odometry factors Pose2 odometry(2.0, 0.0, 0.0); auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); 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; - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); return graph; } diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5e107ea58..a844ee823 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,12 @@ #include #include #include + +#include #include +#include +#include +#include namespace gtsam { @@ -23,8 +28,7 @@ namespace gtsam { * @addtogroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { -protected: - + protected: struct Circle2 { Circle2(const Point2& p, double r) : center(p), radius(r) { @@ -35,11 +39,10 @@ protected: typedef SmartRangeFactor This; - std::vector measurements_; ///< Range measurements - double variance_; ///< variance on noise - -public: + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise + public: /** Default constructor: don't use directly */ SmartRangeFactor() { } @@ -48,7 +51,7 @@ public: * Constructor * @param s standard deviation of range measurement noise */ - SmartRangeFactor(double s) : + explicit SmartRangeFactor(double s) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } @@ -83,9 +86,10 @@ public: /** * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point + * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - //gttic_(triangulate); + // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -96,14 +100,14 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional best_circle; + boost::optional bestCircle2; // loop over all circles - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { // distance between circle centers. double d = distance2(circle1.center, it.center); if (d < 1e-9) - continue; // skip circles that are in the same location + continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); @@ -111,23 +115,28 @@ public: // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; - best_circle = it; + bestCircle2 = it; } } // use best fh to find actual intersection points - std::list intersections = circleCircleIntersection( - circle1.center, best_circle->center, best_fh); + if (bestCircle2 && best_fh) { + auto bestCircleCenter = bestCircle2->center; + std::list intersections = + circleCircleIntersection(circle1.center, bestCircleCenter, best_fh); - // pick winner based on other measurements - double error1 = 0, error2 = 0; - Point2 p1 = intersections.front(), p2 = intersections.back(); - for(const Circle2& it: circles) { - error1 += distance2(it.center, p1); - error2 += distance2(it.center, p2); + // pick winner based on other measurements + double error1 = 0, error2 = 0; + Point2 p1 = intersections.front(), p2 = intersections.back(); + for (const Circle2& it : circles) { + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); + } + return (error1 < error2) ? p1 : p2; + } else { + throw std::runtime_error("triangulate failed"); } - return (error1 < error2) ? p1 : p2; - //gttoc_(triangulate); + // gttoc_(triangulate); } /** @@ -137,19 +146,20 @@ public: boost::optional&> H = boost::none) const { size_t n = size(); if (n < 3) { - if (H) + if (H) { // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) (*H)[j] = Matrix::Zero(3, 1); + } return Z_1x1; } else { Vector error = Z_1x1; // triangulate to get the optimized point - // TODO: Should we have a (better?) variant that does this in relative coordinates ? + // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(x); - // TODO: triangulation should be followed by an optimization given poses + // TODO(dellaert): triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); @@ -168,8 +178,6 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - }; - -} // \namespace gtsam +} // \namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 21b0c5eb7..b7d06b268 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include @@ -35,91 +36,10 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY - }; - - /* - * Parameters for the smart stereo projection factors - */ - struct GTSAM_EXPORT SmartStereoProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - - /// Constructor - SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - 1e-5), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartStereoProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - }; - - +/* + * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams) + */ +typedef SmartProjectionParams SmartStereoProjectionParams; /** * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. @@ -155,14 +75,19 @@ public: /// Vector of cameras typedef CameraSet Cameras; + /// Vector of monocular cameras (stereo treated as 2 monocular) + typedef PinholeCamera MonoCamera; + typedef CameraSet MonoCameras; + typedef MonoCamera::MeasurementVector MonoMeasurements; + /** * Constructor * @param params internal parameters of the smart factors */ SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel), // + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, body_P_sensor), // params_(params), // result_(TriangulationResult::Degenerate()) { } @@ -240,75 +165,28 @@ public: size_t m = cameras.size(); bool retriangulate = decideIfTriangulate(cameras); -// if(!retriangulate) -// std::cout << "retriangulate = false" << std::endl; -// -// bool retriangulate = true; - - if (retriangulate) { -// std::cout << "Retriangulate " << std::endl; - std::vector reprojections; - reprojections.reserve(m); - for(size_t i = 0; i < m; i++) { - reprojections.push_back(cameras[i].backproject(measured_[i])); + // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras + MonoCameras monoCameras; + MonoMeasurements monoMeasured; + for(size_t i = 0; i < m; i++) { + const Pose3 leftPose = cameras[i].pose(); + const Cal3_S2 monoCal = cameras[i].calibration().calibration(); + const MonoCamera leftCamera_i(leftPose,monoCal); + const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); + const Pose3 rightPose = leftPose.compose( left_Pose_right ); + const MonoCamera rightCamera_i(rightPose,monoCal); + const StereoPoint2 zi = measured_[i]; + monoCameras.push_back(leftCamera_i); + monoMeasured.push_back(Point2(zi.uL(),zi.v())); + if(!std::isnan(zi.uR())){ // if right point is valid + monoCameras.push_back(rightCamera_i); + monoMeasured.push_back(Point2(zi.uR(),zi.v())); } - - Point3 pw_sum(0,0,0); - for(const Point3& pw: reprojections) { - pw_sum = pw_sum + pw; - } - // average reprojected landmark - Point3 pw_avg = pw_sum / double(m); - - double totalReprojError = 0; - - // check if it lies in front of all cameras - for(size_t i = 0; i < m; i++) { - const Pose3& pose = cameras[i].pose(); - const Point3& pl = pose.transform_to(pw_avg); - if (pl.z() <= 0) { - result_ = TriangulationResult::BehindCamera(); - return result_; - } - - // check landmark distance - if (params_.triangulation.landmarkDistanceThreshold > 0 && - pl.norm() > params_.triangulation.landmarkDistanceThreshold) { - result_ = TriangulationResult::FarPoint(); - return result_; - } - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { - const StereoPoint2& zi = measured_[i]; - StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); - totalReprojError += reprojectionError.vector().norm(); - } - } // for - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { - result_ = TriangulationResult::Outlier(); - return result_; - } - - if(params_.triangulation.enableEPI) { - try { - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); - } catch(StereoCheiralityException& e) { - if(params_.verboseCheirality) - std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl; - if(params_.throwCheirality) - throw; - result_ = TriangulationResult::BehindCamera(); - return TriangulationResult::BehindCamera(); - } - } - - result_ = TriangulationResult(pw_avg); - - } // if retriangulate + } + if (retriangulate) + result_ = gtsam::triangulateSafe(monoCameras, monoMeasured, + params_.triangulation); return result_; - } /// triangulate @@ -348,17 +226,17 @@ public: } // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). - std::vector Fblocks; + Base::FBlocks Fs; Matrix F, E; Vector b; - computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); // Whiten using noise model - Base::whitenJacobians(Fblocks, E, b); + Base::whitenJacobians(Fs, E, b); // build augmented hessian SymmetricBlockMatrix augmentedHessian = // - Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); + Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping); return boost::make_shared >(this->keys_, augmentedHessian); @@ -482,7 +360,8 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - std::vector& Fblocks, Matrix& E, Vector& b, + FBlocks& Fs, + Matrix& E, Vector& b, const Cameras& cameras) const { if (!result_) { @@ -492,32 +371,32 @@ public: // Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity( // this->measured_.at(0)); */ // -// Base::computeJacobians(Fblocks, E, b, cameras, backProjected); +// Base::computeJacobians(Fs, E, b, cameras, backProjected); } else { // valid result: just return Base version - Base::computeJacobians(Fblocks, E, b, cameras, *result_); + Base::computeJacobians(Fs, E, b, cameras, *result_); } } /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( - std::vector& Fblocks, Matrix& E, Vector& b, + FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); return nonDegenerate; } /// takes values bool triangulateAndComputeJacobiansSVD( - std::vector& Fblocks, Matrix& Enull, Vector& b, + FBlocks& Fs, Matrix& Enull, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_); return nonDegenerate; } @@ -570,6 +449,32 @@ public: } } + /** + * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const + { + // when using stereo cameras, some of the measurements might be missing: + for(size_t i=0; i < cameras.size(); i++){ + const StereoPoint2& z = measured_.at(i); + if(std::isnan(z.uR())) // if the right pixel is invalid + { + if(Fs){ // delete influence of right point on jacobian Fs + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); + + // set the corresponding entry of vector ue to zero + ue(ZDim * i + 1) = 0.0; + } + } + } + /** return the landmark */ TriangulationResult point() const { return result_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 65134cb96..3b4c5e0db 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -66,9 +66,9 @@ public: * @param params internal parameters of the smart factors */ SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel, params) { + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, params, body_P_sensor) { } /** Virtual destructor */ @@ -102,7 +102,7 @@ public: /** * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark * @param K the (known) camera calibration (same for all measurements) */ @@ -161,7 +161,11 @@ public: Base::Cameras cameras; size_t i=0; for(const Key& k: this->keys_) { - const Pose3& pose = values.at(k); + Pose3 pose = values.at(k); + + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 9ae4aa928..e18505af6 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -116,8 +116,8 @@ TEST( SmartRangeFactor, optimization ) { graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.push_back(PriorFactor(1, pose1, priorNoise)); - graph.push_back(PriorFactor(2, pose2, priorNoise)); + graph.emplace_shared >(1, pose1, priorNoise); + graph.emplace_shared >(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 8051e238a..229a49e3f 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,7 +18,7 @@ * @date Sept 2013 */ -// TODO #include +#include #include #include #include @@ -33,8 +33,6 @@ using namespace boost::assign; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); @@ -62,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static double missing_uR = std::numeric_limits::quiet_NaN(); + vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -79,6 +79,35 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, LevenbergMarquardtParams lm_params; +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, params) { + SmartStereoProjectionParams p; + + // check default values and "get" + EXPECT(p.getLinearizationMode() == HESSIAN); + EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY); + EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9); + EXPECT(p.getVerboseCheirality() == false); + EXPECT(p.getThrowCheirality() == false); + + // check "set" + p.setLinearizationMode(JACOBIAN_SVD); + p.setDegeneracyMode(ZERO_ON_DEGENERACY); + p.setRankTolerance(100); + p.setEnableEPI(true); + p.setLandmarkDistanceThreshold(200); + p.setDynamicOutlierRejectionThreshold(3); + p.setRetriangulationThreshold(1e-2); + + EXPECT(p.getLinearizationMode() == JACOBIAN_SVD); + EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5); + EXPECT(p.getTriangulationParameters().enableEPI == true); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5); + EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5); +} + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); @@ -151,6 +180,60 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { //EXPECT(assert_equal(zero(4),actual,1e-8)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); + StereoCamera level_camera(level_pose, K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera level_camera_right(level_pose_right, K2); + + // landmark ~5 meters in front of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + StereoPoint2 level_uv = level_camera.project(landmark); + StereoPoint2 level_uv_right = level_camera_right.project(landmark); + StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v()); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right_missing, x2, K2); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing: + SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + CameraSet cams; + cams += level_camera; + cams += level_camera_right; + TriangulationResult result = factor1.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); + + // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing: + SmartStereoProjectionPoseFactor factor2(model); + StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v()); + factor2.add(level_uv_missing, x1, K2); + factor2.add(level_uv_right_missing, x2, K2); + result = factor2.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); +} + /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) @@ -248,14 +331,12 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; 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.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), @@ -273,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error // get triangulated landmarks from smart factors Point3 landmark1_smart = *smartFactor1->point(); @@ -335,7 +416,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7); LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); Values result2 = optimizer2.optimize(); @@ -344,7 +425,192 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl; } +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { + // camera has some displacement + Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.01, 0., -0.05), Point3(0.1, 0, 0.1)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1.compose(body_P_sensor), K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2.compose(body_P_sensor), K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3.compose(body_P_sensor), K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_l1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_l2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_l3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartStereoProjectionParams smart_params; + smart_params.triangulation.enableEPI = true; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor1->add(measurements_l1, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor2->add(measurements_l2, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor3->add(measurements_l3, views, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + 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)); + + // 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), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; + EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error + + Values result; + gttic_(SmartStereoProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + gttoc_(SmartStereoProjectionPoseFactor); + tictoc_finishedIteration_(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + EXPECT(assert_equal(pose3, result.at(x3))); +} +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + 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); + + // 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(); // + + // These are the poses we want to estimate, from camera measurements + Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); + Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); + Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN) + vector measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo; + for(size_t k=0; k(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, bodyPose1); + gtValues.insert(x2, bodyPose2); + gtValues.insert(x3, bodyPose3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Values values; + values.insert(x1, bodyPose1); + values.insert(x2, bodyPose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, bodyPose3*noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { @@ -390,6 +656,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + // DELETE SOME MEASUREMENTS + StereoPoint2 sp = measurements_cam1[1]; + measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + sp = measurements_cam2[2]; + measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -408,7 +746,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3),1e-7)); } /* *************************************************************************/ @@ -463,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { 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.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), @@ -545,8 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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 @@ -562,7 +900,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); // dynamic outlier rejection is off - EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9); // Factors 1-3 should have valid point, factor 4 should not EXPECT(smartFactor1->point()); @@ -1039,7 +1377,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { vector views; views.push_back(x1); @@ -1072,6 +1410,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; @@ -1082,6 +1423,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), @@ -1098,10 +1442,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - // Hessian is invariant to rotations and translations in the nondegenerate case + // Hessian is invariant to rotations and translations in the degenerate case EXPECT( assert_equal(hessianFactor->information(), +#ifdef GTSAM_USE_EIGEN_MKL + hessianFactorRotTran->information(), 1e-5)); +#else hessianFactorRotTran->information(), 1e-6)); +#endif } /* ************************************************************************* */ diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m old mode 100644 new mode 100755 index 2900aed99..ad8d71c4c --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -7,8 +7,10 @@ if ~exist('frames','var'), scale=[]; end import gtsam.* +Pose3Values = gtsam.utilities.allPose3s(values); + haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); +keys = KeyVector(Pose3Values.keys); holdstate = ishold; hold on @@ -16,49 +18,48 @@ hold on % Plot poses and covariance matrices lastIndex = []; for i = 0:keys.size-1 - key = keys.at(i); - try - x = values.atPose3(key); - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - try - lastPose = values.atPose3(lastKey); - plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - else - P = []; - end - gtsam.plotPose3(lastPose, P, scale); - catch err - % warning(['no Pose3 at ' lastKey]); - end - end - lastIndex = i; - catch - % warning(['no Pose3 at ' key]); - end - - % Draw final pose + key = keys.at(i); + try + x = Pose3Values.atPose3(key); if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - try - lastPose = values.atPose3(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - else - P = []; - end - gtsam.plotPose3(lastPose, P, scale); - catch - % warning(['no Pose3 at ' lastIndex]); + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + try + lastPose = Pose3Values.atPose3(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; end + gtsam.plotPose3(lastPose, P, scale); + catch err + % warning(['no Pose3 at ' lastKey]); + end end - - if ~holdstate - hold off + lastIndex = i; + catch + warning(['no Pose3 at ' key]); + end +end + +% Draw final pose +if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + try + lastPose = Pose3Values.atPose3(lastKey); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; end - -end \ No newline at end of file + gtsam.plotPose3(lastPose, P, scale); + catch + % warning(['no Pose3 at ' lastIndex]); + end +end + +if ~holdstate + hold off +end diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt index 6f74f9806..a1691be32 100644 --- a/matlab/README-gtsam-toolbox.txt +++ b/matlab/README-gtsam-toolbox.txt @@ -7,8 +7,14 @@ http://borg.cc.gatech.edu/projects/gtsam ================================================================================ This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ -library. +library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake. +The interface to the toolbox is generated automatically by the wrap +tool which directly parses C++ header files. The tool generates matlab +proxy objects together with all the native functions for wrapping and +unwrapping scalar and non scalar types and objects. The interface +generated by the wrap tool also redirects the standard output stream +(cout) to matlab's console. ---------------------------------------- Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m old mode 100644 new mode 100755 index 32f61e47f..c80b6ec3e --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -42,6 +42,13 @@ sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadat g = [0;0;-9.8]; w_coriolis = [0;0;0]; +IMU_params = PreintegrationParams(g); + +IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3)); +IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3)); +IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3)); +IMU_params.setOmegaCoriolis(w_coriolis); + %% Solver object isamParams = ISAM2Params; isamParams.setFactorization('CHOLESKY'); @@ -65,7 +72,7 @@ for measurementIndex = firstGPSPose:length(GPS_data) currentVelKey = symbol('v',measurementIndex); currentBiasKey = symbol('b',measurementIndex); t = GPS_data(measurementIndex, 1).Time; - + if measurementIndex == firstGPSPose %% Create initial estimate and prior on initial pose, velocity, and biases newValues.insert(currentPoseKey, currentPoseGlobal); @@ -78,10 +85,8 @@ for measurementIndex = firstGPSPose:length(GPS_data) t_previous = GPS_data(measurementIndex-1, 1).Time; %% Summarize IMU data between the previous GPS measurement and now IMUindices = find(IMUtimes >= t_previous & IMUtimes <= t); - - currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + + currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias); for imuIndex = IMUindices accMeas = [ IMU_data(imuIndex).accelX; IMU_data(imuIndex).accelY; IMU_data(imuIndex).accelZ ]; @@ -94,8 +99,8 @@ for measurementIndex = firstGPSPose:length(GPS_data) newFactors.add(ImuFactor( ... currentPoseKey-1, currentVelKey-1, ... currentPoseKey, currentVelKey, ... - currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)); - + currentBiasKey, currentSummarizedMeasurement)); + % Bias evolution as given in the IMU metadata newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Diagonal.Sigmas(sqrt(numel(IMUindices)) * sigma_between_b))); @@ -119,10 +124,13 @@ for measurementIndex = firstGPSPose:length(GPS_data) isam.update(newFactors, newValues); newFactors = NonlinearFactorGraph; newValues = Values; + + result = isam.calculateEstimate(); if rem(measurementIndex,10)==0 % plot every 10 time steps cla; - plot3DTrajectory(isam.calculateEstimate, 'g-'); + + plot3DTrajectory(result, 'g-'); title('Estimated trajectory using ISAM2 (IMU+GPS)') xlabel('[m]') ylabel('[m]') @@ -131,12 +139,13 @@ for measurementIndex = firstGPSPose:length(GPS_data) drawnow; end % ======================================================================= - currentPoseGlobal = isam.calculateEstimate(currentPoseKey); - currentVelocityGlobal = isam.calculateEstimate(currentVelKey); - currentBias = isam.calculateEstimate(currentBiasKey); + + currentPoseGlobal = result.atPose3(currentPoseKey); + currentVelocityGlobal = result.atVector(currentVelKey); + currentBias = result.atConstantBias(currentBiasKey); end end - + end % end main loop disp('-- Reached end of sensor data') diff --git a/package.xml b/package.xml index f7b3e0dc5..5b50b5af9 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ gtsam - 3.1.0 + 3.2.1 gtsam Frank Dellaert diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index 182533672..bb5845a3c 100755 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -18,7 +18,7 @@ fi echo "Platform is ${platform}" -# Check for empty directory +# Check for empty diectory if [ ! -z "`ls`" ]; then echo "Please run this script from an empty build directory" exit 1 diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 2e95ea33a..2d7ac72f7 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from gtsampy import * +from _gtsampy import * diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt index fd022dd17..689354b4e 100644 --- a/python/handwritten/CMakeLists.txt +++ b/python/handwritten/CMakeLists.txt @@ -12,7 +12,7 @@ endforeach() add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) set_target_properties(gtsam_python PROPERTIES - OUTPUT_NAME gtsampy + OUTPUT_NAME _gtsampy PREFIX "" ${build_type_toupper}_POSTFIX "" SKIP_BUILD_RPATH TRUE @@ -31,11 +31,11 @@ target_link_libraries(gtsam_python # Cause the library to be output in the correct directory. # TODO: Change below to work on different systems (currently works only with Linux) -set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_libgtsam_python.so) +set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_gtsampy.so) add_custom_command( OUTPUT ${output_path} DEPENDS gtsam_python COMMAND ${CMAKE_COMMAND} -E copy $ ${output_path} - COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" + COMMENT "Copying extension module to python/gtsam/_gtsampy.so" ) add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path}) \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 94dc10e56..8fa7e0fdd 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/python/handwritten/exportgtsam.cpp @@ -62,7 +62,7 @@ void registerNumpyEigenConversions(); //-----------------------------------// -BOOST_PYTHON_MODULE(gtsampy){ +BOOST_PYTHON_MODULE(_gtsampy){ // NOTE: We need to call import_array1() instead of import_array() to support both python 2 // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp index da98783e2..4153ecf06 100644 --- a/python/handwritten/geometry/PinholeBaseK.cpp +++ b/python/handwritten/geometry/PinholeBaseK.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -36,31 +36,38 @@ struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; -Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +// Function pointers to disambiguate project() calls +Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; -// function pointers to desambiguate range() calls +// function pointers to disambiguate range() calls double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range; double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; -void exportPinholeBaseK(){ +// wrap projectSafe in a function that returns None or a tuple +// TODO(frank): find out how to return an ndarray instead +object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) { + auto result = camera.projectSafe(p); + if (result.second) + return make_tuple(result.first.x(), result.first.y()); + else + return object(); +} +void exportPinholeBaseK() { class_("PinholeBaseKCal3_S2", no_init) - .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy()) - .def("project", project1) - .def("project", project2, project_overloads()) - .def("project", project3, project_overloads()) - .def("backproject", &PinholeBaseKCal3_S2::backproject) - .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) - .def("range", range1, range_overloads()) - .def("range", range2, range_overloads()) - .def("range", range3, range_overloads()) - ; - -} \ No newline at end of file + .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), + return_value_policy()) + .def("project_safe", make_function(project_safe)) + .def("project", project1, project_overloads()) + .def("project", project2, project_overloads()) + .def("backproject", &PinholeBaseKCal3_S2::backproject) + .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("range", range3, range_overloads()); +} diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index f778ea4e0..551f2f60c 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -53,6 +53,9 @@ void exportPose3(){ // function pointers to desambiguate range() calls double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + // function pointers to desambiguate bearing() calls + Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const = &Pose3::bearing; + Unit3 (Pose3::*bearing2)(const Pose3 &, OptionalJacobian<2,6>, OptionalJacobian<2,6>) const = &Pose3::bearing; class_("Pose3") .def(init<>()) @@ -65,7 +68,6 @@ void exportPose3(){ .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol"))) .def("identity", &Pose3::identity) .staticmethod("identity") - .def("bearing", &Pose3::bearing) .def("matrix", &Pose3::matrix) .def("transform_from", &Pose3::transform_from, transform_from_overloads(args("point", "H1", "H2"))) @@ -88,5 +90,6 @@ void exportPose3(){ .def("between", between2, between_overloads()) .def("range", range1, range_overloads()) .def("range", range2, range_overloads()) - .def("bearing", &Pose3::bearing, bearing_overloads()); + .def("bearing", bearing1, bearing_overloads()) + .def("bearing", bearing2, bearing_overloads()); } diff --git a/python/setup.py.in b/python/setup.py.in index d3b5fcde4..8b2de7352 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -11,5 +11,5 @@ setup(name='gtsam', package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir - data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_gtsampy.so'])], # location of .so file relative to setup.py ) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 8b2aa3ae7..984728ebf 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -317,8 +317,16 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) 5, 0, 6, 0, -11, -12 ).finished(); + Matrix actualJointJ = joint.augmentedJacobian(); + // PR 315: sign of rows in joint are immaterial + if (signbit(expectedJointJ(0, 2)) != signbit(actualJointJ(0, 2))) + expectedJointJ.row(0) = -expectedJointJ.row(0); + + if (signbit(expectedJointJ(1, 2)) != signbit(actualJointJ(1, 2))) + expectedJointJ.row(1) = -expectedJointJ.row(1); + EXPECT(assert_equal(expectedJointJ, actualJointJ)); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f56b458be..d6ca896dd 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -304,7 +304,7 @@ TEST(ISAM2, AddFactorsStep1) FactorIndices actualNewFactorIndices; - ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); @@ -843,7 +843,7 @@ TEST(ISAM2, calculate_nnz) { ISAM2 isam = createSlamlikeISAM2(); int expected = 241; - int actual = calculate_nnz(isam.roots().front()); + int actual = isam.roots().front()->calculate_nnz(); EXPECT_LONGS_EQUAL(expected, actual); } diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 410617cbf..95caaaf9c 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -51,7 +51,7 @@ TEST(PinholeCamera, BAL) { for (size_t j = 0; j < db.number_tracks(); j++) { for (const SfM_Measurement& m: db.tracks[j].measurements) - graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + graph.emplace_shared(m.second, unit2, m.first, P(j)); } Values initial = initialCamerasAndPointsEstimate(db); diff --git a/tests/testLie.cpp b/tests/testLie.cpp index a134a899c..2d8a0b975 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -102,6 +102,33 @@ TEST( testProduct, inverse ) { EXPECT(assert_equal(numericH1, actH1, tol)); } +/* ************************************************************************* */ +Product expmap_proxy(const Vector5& vec) { + return Product::Expmap(vec); +} +TEST( testProduct, Expmap ) { + Vector5 vec; + vec << 1, 2, 0.1, 0.2, 0.3; + + Matrix actH; + Product::Expmap(vec, actH); + Matrix numericH = numericalDerivative11(expmap_proxy, vec); + EXPECT(assert_equal(numericH, actH, tol)); +} + +/* ************************************************************************* */ +Vector5 logmap_proxy(const Product& p) { + return Product::Logmap(p); +} +TEST( testProduct, Logmap ) { + Product state(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH; + Product::Logmap(state, actH); + Matrix numericH = numericalDerivative11(logmap_proxy, state); + EXPECT(assert_equal(numericH, actH, tol)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 617a8cc1c..c06d10beb 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -16,7 +17,10 @@ #include #include +#include +#include +using namespace std; using namespace gtsam; const double tol=1e-5; @@ -228,6 +232,93 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { EXPECT(assert_equal(expected.at(lm3), actualQR.at(lm3), tol)); } +/* ************************************************************************* */ +TEST(testNonlinearISAM, loop_closures ) { + int relinearizeInterval = 100; + NonlinearISAM isam(relinearizeInterval); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + vector lines; + lines.emplace_back("VERTEX2 0 0.000000 0.000000 0.000000"); + lines.emplace_back("EDGE2 1 0 1.030390 0.011350 -0.012958"); + lines.emplace_back("VERTEX2 1 1.030390 0.011350 -0.012958"); + lines.emplace_back("EDGE2 2 1 1.013900 -0.058639 -0.013225"); + lines.emplace_back("VERTEX2 2 2.043445 -0.060422 -0.026183"); + lines.emplace_back("EDGE2 3 2 1.027650 -0.007456 0.004833"); + lines.emplace_back("VERTEX2 3 3.070548 -0.094779 -0.021350"); + lines.emplace_back("EDGE2 4 3 -0.012016 1.004360 1.566790"); + lines.emplace_back("VERTEX2 4 3.079976 0.909609 1.545440"); + lines.emplace_back("EDGE2 5 4 1.016030 0.014565 -0.016304"); + lines.emplace_back("VERTEX2 5 3.091176 1.925681 1.529136"); + lines.emplace_back("EDGE2 6 5 1.023890 0.006808 0.010981"); + lines.emplace_back("VERTEX2 6 3.127018 2.948966 1.540117"); + lines.emplace_back("EDGE2 7 6 0.957734 0.003159 0.010901"); + lines.emplace_back("VERTEX2 7 3.153237 3.906347 1.551018"); + lines.emplace_back("EDGE2 8 7 -1.023820 -0.013668 -3.093240"); + lines.emplace_back("VERTEX2 8 3.146655 2.882457 -1.542222"); + lines.emplace_back("EDGE2 9 8 1.023440 0.013984 -0.007802"); + lines.emplace_back("EDGE2 9 5 0.033943 0.032439 -3.127400"); + lines.emplace_back("VERTEX2 9 3.189873 1.859834 -1.550024"); + lines.emplace_back("EDGE2 10 9 1.003350 0.022250 0.023491"); + lines.emplace_back("EDGE2 10 3 0.044020 0.988477 -1.563530"); + lines.emplace_back("VERTEX2 10 3.232959 0.857162 -1.526533"); + lines.emplace_back("EDGE2 11 10 0.977245 0.019042 -0.028623"); + lines.emplace_back("VERTEX2 11 3.295225 -0.118283 -1.555156"); + lines.emplace_back("EDGE2 12 11 -0.996880 -0.025512 -3.126915"); + lines.emplace_back("VERTEX2 12 3.254125 0.878076 1.601114"); + lines.emplace_back("EDGE2 13 12 0.990646 0.018396 -0.016519"); + lines.emplace_back("VERTEX2 13 3.205708 1.867709 1.584594"); + lines.emplace_back("EDGE2 14 13 0.945873 0.008893 -0.002602"); + lines.emplace_back("EDGE2 14 8 0.015808 0.021059 3.128310"); + lines.emplace_back("VERTEX2 14 3.183765 2.813370 1.581993"); + lines.emplace_back("EDGE2 15 14 1.000010 0.006428 0.028234"); + lines.emplace_back("EDGE2 15 7 -0.014728 -0.001595 -0.019579"); + lines.emplace_back("VERTEX2 15 3.166141 3.813245 1.610227"); + + auto model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); + + // Loop over the different poses, adding the observations to iSAM incrementally + for (const string& str : lines) { + // scan the tag + string tag; + istringstream is(str); + if (!(is >> tag)) + break; + + // Check if vertex + const auto indexedPose = parseVertex(is, tag); + if (indexedPose) { + Key id = indexedPose->first; + initialEstimate.insert(Symbol('x', id), indexedPose->second); + 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); + } else { + isam.update(graph, initialEstimate); + + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } + } + + // check if edge + const auto betweenPose = parseEdge(is, tag); + if (betweenPose) { + Key id1, id2; + tie(id1, id2) = betweenPose->first; + graph.emplace_shared >(Symbol('x', id2), + Symbol('x', id1), betweenPose->second, model); + } + } + EXPECT_LONGS_EQUAL(16, isam.estimate().size()) +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 9ddbbb1b2..3d30d6880 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,13 +20,12 @@ #include #include #include -#include -#include #include #include #include #include #include +#include #include #include @@ -394,19 +393,20 @@ class IterativeLM: public LevenbergMarquardtOptimizer { /// Solver specific parameters ConjugateGradientParameters cgParams_; + Values initial_; public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : - LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { + LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) { } /// Solve that uses conjugate gradient virtual VectorValues solve(const GaussianFactorGraph &gfg, - const Values& initial, const NonlinearOptimizerParams& params) const { - VectorValues zeros = initial.zeroVectors(); + const NonlinearOptimizerParams& params) const { + VectorValues zeros = initial_.zeroVectors(); return conjugateGradientDescent(gfg, zeros, cgParams_); } }; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1e695b3a..be1104b1a 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -40,7 +40,7 @@ int main(int argc, char* argv[]) { for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; - graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); + graph.emplace_shared(z, gNoiseModel, C(i), P(j)); } } diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 52d9ca0b5..913c11284 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -17,6 +17,7 @@ **/ #include "Argument.h" +#include "Class.h" #include @@ -62,29 +63,28 @@ string Argument::matlabClass(const string& delim) const { return result + type.name(); } -/* ************************************************************************* */ -bool Argument::isScalar() const { - return (type.name() == "bool" || type.name() == "char" - || type.name() == "unsigned char" || type.name() == "int" - || type.name() == "size_t" || type.name() == "double"); -} - /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; string cppType = type.qualifiedName("::"); string matlabUniqueType = type.qualifiedName(); + bool isNotScalar = !type.isScalar(); + + // We cannot handle scalar non const references + if (!isNotScalar && is_ref && !is_const) { + throw std::runtime_error("Cannot unwrap a scalar non-const reference"); + } if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; - else if (is_ref && type.category != Qualified::EIGEN) + else if (is_ref && isNotScalar && type.category != Qualified::EIGEN) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; else - // Not a pointer or a reference: emit an "unwrap" call + // Not a pointer, or a reference to a scalar type. Therefore, emit an "unwrap" call // unwrap is specified in matlab.h as a series of template specializations // that know how to unpack the expected MATLAB object // example: double tol = unwrap< double >(in[2]); @@ -92,7 +92,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if( (is_ptr || is_ref) && type.category != Qualified::EIGEN) + if( (is_ptr || is_ref) && isNotScalar && type.category != Qualified::EIGEN) file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } @@ -104,6 +104,54 @@ void Argument::proxy_check(FileWriter& proxyFile, const string& s) const { proxyFile.oss << " && size(" << s << ",2)==1"; } +/* ************************************************************************* */ +void Argument::emit_cython_pxd( + FileWriter& file, const std::string& className, + const std::vector& templateArgs) const { + string cythonType = type.pxdClassName(); + if (cythonType == "This") cythonType = className; + else if (type.isEigen()) + cythonType = "const " + cythonType + "&"; + else if (type.match(templateArgs)) + cythonType = type.name(); + + // add modifier + if (!type.isEigen()) { + if (is_ptr) cythonType = "shared_ptr[" + cythonType + "]&"; + if (is_ref) cythonType = cythonType + "&"; + if (is_const) cythonType = "const " + cythonType; + } + + file.oss << cythonType << " " << name; +} + +/* ************************************************************************* */ +void Argument::emit_cython_pyx(FileWriter& file) const { + file.oss << type.pyxArgumentType() << " " << name; +} + +/* ************************************************************************* */ +std::string Argument::pyx_convertEigenTypeAndStorageOrder() const { + if (!type.isEigen()) + return ""; + return name + " = " + name + ".astype(float, order=\'F\', copy=False)"; +} + +/* ************************************************************************* */ +std::string Argument::pyx_asParam() const { + string cythonType = type.pxdClassName(); + string cythonVar; + if (type.isNonBasicType()) { + cythonVar = name + "." + type.shared_pxd_obj_in_pyx(); + if (!is_ptr) cythonVar = "deref(" + cythonVar + ")"; + } else if (type.isEigen()) { + cythonVar = "<" + cythonType + ">" + "(Map[" + cythonType + "](" + name + "))"; + } else { + cythonVar = name; + } + return cythonVar; +} + /* ************************************************************************* */ string ArgumentList::types() const { string str; @@ -154,7 +202,7 @@ string ArgumentList::names() const { /* ************************************************************************* */ bool ArgumentList::allScalar() const { for(Argument arg: *this) - if (!arg.isScalar()) + if (!arg.type.isScalar()) return false; return true; } @@ -183,6 +231,69 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << ")"; } +/* ************************************************************************* */ +void ArgumentList::emit_cython_pxd( + FileWriter& file, const std::string& className, + const std::vector& templateArgs) const { + for (size_t j = 0; j(__params[" + std::to_string(j) + "])\n"; + return s; +} + /* ************************************************************************* */ void ArgumentList::proxy_check(FileWriter& proxyFile) const { // Check nr of arguments diff --git a/wrap/Argument.h b/wrap/Argument.h index fd7e82061..0a4ebba9d 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -39,6 +39,12 @@ struct Argument { type(t), name(n), is_const(false), is_ref(false), is_ptr(false) { } + bool isSameSignature(const Argument& other) const { + return type == other.type + && is_const == other.is_const && is_ref == other.is_ref + && is_ptr == other.is_ptr; + } + bool operator==(const Argument& other) const { return type == other.type && name == other.name && is_const == other.is_const && is_ref == other.is_ref @@ -50,9 +56,6 @@ struct Argument { /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; - /// Check if will be unwrapped using scalar login in wrap/matlab.h - bool isScalar() const; - /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; @@ -62,6 +65,16 @@ struct Argument { */ void proxy_check(FileWriter& proxyFile, const std::string& s) const; + /** + * emit arguments for cython pxd + * @param file output stream + */ + void emit_cython_pxd(FileWriter& file, const std::string& className, + const std::vector& templateArgs) const; + void emit_cython_pyx(FileWriter& file) const; + std::string pyx_asParam() const; + std::string pyx_convertEigenTypeAndStorageOrder() const; + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") << (arg.is_ref ? "&" : ""); @@ -87,6 +100,12 @@ struct ArgumentList: public std::vector { ArgumentList expandTemplate(const TemplateSubstitution& ts) const; + bool isSameSignature(const ArgumentList& other) const { + for(size_t i = 0; i { */ void emit_prototype(FileWriter& file, const std::string& name) const; + /** + * emit arguments for cython pxd + * @param file output stream + */ + void emit_cython_pxd(FileWriter& file, const std::string& className, + const std::vector& templateArgs) const; + void emit_cython_pyx(FileWriter& file) const; + std::string pyx_asParams() const; + std::string pyx_paramsList() const; + std::string pyx_castParamsToPythonType(const std::string& indent) const; + std::string pyx_convertEigenTypeAndStorageOrder(const std::string& indent) const; + /** * emit checking arguments to MATLAB proxy * @param proxyFile output stream diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 3a12290eb..bff1d36b2 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -19,10 +19,15 @@ #include "Class.h" #include "utilities.h" #include "Argument.h" +#include #include #include #include +#include +#include +#include +#include #include #include @@ -58,14 +63,14 @@ static void handleException(const out_of_range& oor, } /* ************************************************************************* */ -Method& Class::mutableMethod(Str key) { - try { - return methods_.at(key); - } catch (const out_of_range& oor) { - handleException(oor, methods_); - throw runtime_error("Internal error in wrap"); - } -} +// Method& Class::mutableMethod(Str key) { +// try { +// return methods_.at(key); +// } catch (const out_of_range& oor) { +// handleException(oor, methods_); +// throw runtime_error("Internal error in wrap"); +// } +// } /* ************************************************************************* */ const Method& Class::method(Str key) const { @@ -309,6 +314,8 @@ vector Class::expandTemplate(Str templateArg, inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + ">"; + inst.templateInstTypeList.push_back(instName); + inst.templateClass = *this; result.push_back(inst); } return result; @@ -339,9 +346,13 @@ void Class::addMethod(bool verbose, bool is_const, Str methodName, const Template& tmplate) { // Check if templated if (tmplate.valid()) { + templateMethods_[methodName].addOverload(methodName, argumentList, + returnValue, is_const, + tmplate.argName(), verbose); // Create method to expand // For all values of the template argument, create a new method for(const Qualified& instName: tmplate.argValues()) { + const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); @@ -353,36 +364,44 @@ void Class::addMethod(bool verbose, bool is_const, Str methodName, methods_[expandedMethodName].addOverload(methodName, expandedArgs, expandedRetVal, is_const, instName, verbose); } - } else + } else { // just add overload methods_[methodName].addOverload(methodName, argumentList, returnValue, is_const, boost::none, verbose); + nontemplateMethods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); + } } /* ************************************************************************* */ -void Class::erase_serialization() { - Methods::iterator it = methods_.find("serializable"); - if (it != methods_.end()) { +void Class::erase_serialization(Methods& methods) { + Methods::iterator it = methods.find("serializable"); + if (it != methods.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; #else // cout << "Ignoring serializable() flag in class " << name << endl; #endif - methods_.erase(it); + methods.erase(it); } - it = methods_.find("serialize"); - if (it != methods_.end()) { + it = methods.find("serialize"); + if (it != methods.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; hasSerialization = true; #else // cout << "Ignoring serialize() flag in class " << name << endl; #endif - methods_.erase(it); + methods.erase(it); } } +void Class::erase_serialization() { + erase_serialization(methods_); + erase_serialization(nontemplateMethods_); +} + /* ************************************************************************* */ void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { @@ -422,6 +441,56 @@ void Class::appendInheritedMethods(const Class& cls, } } +/* ************************************************************************* */ +void Class::removeInheritedNontemplateMethods(vector& classes) { + if (!parentClass) return; + // Find parent + auto parentIt = std::find_if(classes.begin(), classes.end(), + [&](const Class& cls) { return cls.name() == parentClass->name(); }); + if (parentIt == classes.end()) return; // ignore if parent not found + Class& parent = *parentIt; + + // Only check nontemplateMethods_ + for(const string& methodName: nontemplateMethods_ | boost::adaptors::map_keys) { + // check if the method exists in its parent + // Check against parent's methods_ because all the methods of grand + // parent and grand-grand-parent, etc. are already included there + // This is to avoid looking into higher level grand parents... + auto it = parent.methods_.find(methodName); + if (it == parent.methods_.end()) continue; // if not: ignore! + + Method& parentMethod = it->second; + Method& method = nontemplateMethods_[methodName]; + // check if they have the same modifiers (const/static/templateArgs) + if (!method.isSameModifiers(parentMethod)) continue; // if not: ignore + + // check and remove duplicate overloads + auto methodOverloads = boost::combine(method.returnVals_, method.argLists_); + auto parentMethodOverloads = boost::combine(parentMethod.returnVals_, parentMethod.argLists_); + auto result = boost::remove_if( + methodOverloads, + [&](boost::tuple const& overload) { + bool found = std::find_if( + parentMethodOverloads.begin(), + parentMethodOverloads.end(), + [&](boost::tuple const& + parentOverload) { + return overload.get<0>() == parentOverload.get<0>() && + overload.get<1>().isSameSignature(parentOverload.get<1>()); + }) != parentMethodOverloads.end(); + return found; + }); + // remove all duplicate overloads + method.returnVals_.erase(boost::get<0>(result.get_iterator_tuple()), + method.returnVals_.end()); + method.argLists_.erase(boost::get<1>(result.get_iterator_tuple()), + method.argLists_.end()); + } + // [Optional] remove the entire method if it has no overload + for (auto it = nontemplateMethods_.begin(), ite = nontemplateMethods_.end(); it != ite;) + if (it->second.nrOverloads() == 0) it = nontemplateMethods_.erase(it); else ++it; +} + /* ************************************************************************* */ string Class::getTypedef() const { string result; @@ -660,3 +729,153 @@ void Class::python_wrapper(FileWriter& wrapperFile) const { } /* ************************************************************************* */ +void Class::emit_cython_pxd(FileWriter& pxdFile) const { + pxdFile.oss << "cdef extern from \"" << includeFile << "\""; + string ns = qualifiedNamespaces("::"); + if (!ns.empty()) + pxdFile.oss << " namespace \"" << ns << "\""; + pxdFile.oss << ":" << endl; + pxdFile.oss << " cdef cppclass " << pxdClassName() << " \"" << qualifiedName("::") << "\""; + if (templateArgs.size()>0) { + pxdFile.oss << "["; + for(size_t i = 0; ipxdClassName() << ")"; + pxdFile.oss << ":\n"; + + constructor.emit_cython_pxd(pxdFile, *this); + if (constructor.nrOverloads()>0) pxdFile.oss << "\n"; + + for(const StaticMethod& m: static_methods | boost::adaptors::map_values) + m.emit_cython_pxd(pxdFile, *this); + if (static_methods.size()>0) pxdFile.oss << "\n"; + + for(const Method& m: nontemplateMethods_ | boost::adaptors::map_values) + m.emit_cython_pxd(pxdFile, *this); + + for(const TemplateMethod& m: templateMethods_ | boost::adaptors::map_values) + m.emit_cython_pxd(pxdFile, *this); + size_t numMethods = constructor.nrOverloads() + static_methods.size() + + methods_.size() + templateMethods_.size(); + if (numMethods == 0) + pxdFile.oss << " pass\n"; +} +/* ************************************************************************* */ +void Class::emit_cython_wrapper_pxd(FileWriter& pxdFile) const { + pxdFile.oss << "\ncdef class " << pyxClassName(); + if (getParent()) + pxdFile.oss << "(" << getParent()->pyxClassName() << ")"; + pxdFile.oss << ":\n"; + pxdFile.oss << " cdef " << shared_pxd_class_in_pyx() << " " + << shared_pxd_obj_in_pyx() << "\n"; + // cyCreateFromShared + pxdFile.oss << " @staticmethod\n"; + pxdFile.oss << " cdef " << pyxClassName() << " cyCreateFromShared(const " + << shared_pxd_class_in_pyx() << "& other)\n"; + for(const StaticMethod& m: static_methods | boost::adaptors::map_values) + m.emit_cython_wrapper_pxd(pxdFile, *this); + if (static_methods.size()>0) pxdFile.oss << "\n"; +} + +/* ************************************************************************* */ +void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, + const std::string& cySharedObj, + const std::vector& allClasses) const { + if (parentClass) { + pyxFile.oss << pyObj << "." << parentClass->shared_pxd_obj_in_pyx() << " = " + << "<" << parentClass->shared_pxd_class_in_pyx() << ">(" + << cySharedObj << ")\n"; + // Find the parent class with name "parentClass" and point its cython obj + // to the same pointer + auto parent_it = find_if(allClasses.begin(), allClasses.end(), + [this](const Class& cls) { + return cls.pxdClassName() == + this->parentClass->pxdClassName(); + }); + if (parent_it == allClasses.end()) { + cerr << "Can't find parent class: " << parentClass->pxdClassName(); + throw std::runtime_error("Parent class not found!"); + } + parent_it->pyxInitParentObj(pyxFile, pyObj, cySharedObj, allClasses); + } +} + +/* ************************************************************************* */ +void Class::pyxDynamicCast(FileWriter& pyxFile, const Class& curLevel, + const std::vector& allClasses) const { + std::string me = this->pyxClassName(), sharedMe = this->shared_pxd_class_in_pyx(); + if (curLevel.parentClass) { + std::string parent = curLevel.parentClass->pyxClassName(), + parentObj = curLevel.parentClass->shared_pxd_obj_in_pyx(), + parentCythonClass = curLevel.parentClass->pxd_class_in_pyx(); + pyxFile.oss << "def dynamic_cast_" << me << "_" << parent << "(" << parent + << " parent):\n"; + pyxFile.oss << " try:\n"; + pyxFile.oss << " return " << me << ".cyCreateFromShared(<" << sharedMe + << ">dynamic_pointer_cast[" << pxd_class_in_pyx() << "," + << parentCythonClass << "](parent." << parentObj + << "))\n"; + pyxFile.oss << " except:\n"; + pyxFile.oss << " raise TypeError('dynamic cast failed!')\n"; + // Move up higher to one level: Find the parent class with name "parentClass" + auto parent_it = find_if(allClasses.begin(), allClasses.end(), + [&curLevel](const Class& cls) { + return cls.pxdClassName() == + curLevel.parentClass->pxdClassName(); + }); + if (parent_it == allClasses.end()) { + cerr << "Can't find parent class: " << parentClass->pxdClassName(); + throw std::runtime_error("Parent class not found!"); + } + pyxDynamicCast(pyxFile, *parent_it, allClasses); + } +} + +/* ************************************************************************* */ +void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector& allClasses) const { + pyxFile.oss << "cdef class " << pyxClassName(); + if (parentClass) pyxFile.oss << "(" << parentClass->pyxClassName() << ")"; + pyxFile.oss << ":\n"; + + // __init___ + pyxFile.oss << " def __init__(self, *args, **kwargs):\n"; + pyxFile.oss << " cdef list __params\n"; + pyxFile.oss << " self." << shared_pxd_obj_in_pyx() << " = " << shared_pxd_class_in_pyx() << "()\n"; + pyxFile.oss << " if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):\n return\n"; + + // Constructors + constructor.emit_cython_pyx(pyxFile, *this); + pyxFile.oss << " if (self." << shared_pxd_obj_in_pyx() << ".use_count()==0):\n"; + pyxFile.oss << " raise TypeError('" << pyxClassName() + << " construction failed!')\n"; + pyxInitParentObj(pyxFile, " self", "self." + shared_pxd_obj_in_pyx(), allClasses); + pyxFile.oss << "\n"; + + // cyCreateFromShared + pyxFile.oss << " @staticmethod\n"; + pyxFile.oss << " cdef " << pyxClassName() << " cyCreateFromShared(const " + << shared_pxd_class_in_pyx() << "& other):\n" + << " if other.get() == NULL:\n" + << " raise RuntimeError('Cannot create object from a nullptr!')\n" + << " cdef " << pyxClassName() << " return_value = " << pyxClassName() << "(cyCreateFromShared=True)\n" + << " return_value." << shared_pxd_obj_in_pyx() << " = other\n"; + pyxInitParentObj(pyxFile, " return_value", "other", allClasses); + pyxFile.oss << " return return_value" << "\n\n"; + + for(const StaticMethod& m: static_methods | boost::adaptors::map_values) + m.emit_cython_pyx(pyxFile, *this); + if (static_methods.size()>0) pyxFile.oss << "\n"; + + for(const Method& m: methods_ | boost::adaptors::map_values) + m.emit_cython_pyx(pyxFile, *this); + + pyxDynamicCast(pyxFile, *this, allClasses); + + pyxFile.oss << "\n\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index e69c38f41..910ecde57 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -25,6 +25,7 @@ #include "Deconstructor.h" #include "Method.h" #include "StaticMethod.h" +#include "TemplateMethod.h" #include "TypeAttributesTable.h" #ifdef __GNUC__ @@ -54,12 +55,15 @@ public: typedef const std::string& Str; typedef std::map Methods; typedef std::map StaticMethods; + typedef std::map TemplateMethods; private: boost::optional parentClass; ///< The *single* parent - Methods methods_; ///< Class methods - Method& mutableMethod(Str key); + Methods methods_; ///< Class methods, including all expanded/instantiated template methods -- to be serialized to matlab and Python classes in Cython pyx + Methods nontemplateMethods_; ///< only nontemplate methods -- to be serialized into Cython pxd + TemplateMethods templateMethods_; ///< only template methods -- to be serialized into Cython pxd + // Method& mutableMethod(Str key); public: @@ -68,12 +72,17 @@ public: // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] + std::vector templateInstTypeList; ///< the original typelist used to instantiate this class from a template. + ///< Empty if it's not an instantiation. Needed for template classes in Cython pxd. + boost::optional templateClass = boost::none; ///< qualified name of the original template class from which this class was instantiated. + ///< boost::none if not an instantiation. Needed for template classes in Cython pxd. bool isVirtual; ///< Whether the class is part of a virtual inheritance chain bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports bool hasSerialization; ///< Whether we should create the serialization functions Constructor constructor; ///< Class constructors Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object bool verbose_; ///< verbose flag + std::string includeFile; /// Constructor creates an empty class Class(bool verbose = true) : @@ -81,9 +90,19 @@ public: false), deconstructor(verbose), verbose_(verbose) { } + Class(const std::string& name, bool verbose = true) + : Qualified(name, Qualified::Category::CLASS), + parentClass(boost::none), + isVirtual(false), + isSerializable(false), + hasSerialization(false), + deconstructor(verbose), + verbose_(verbose) {} + void assignParent(const Qualified& parent); boost::optional qualifiedParent() const; + boost::optional getParent() const { return parentClass; } size_t nrMethods() const { return methods_.size(); @@ -92,7 +111,7 @@ public: const Method& method(Str key) const; bool exists(Str name) const { - return methods_.find(name) != methods_.end(); + return methods_.find(name) != methods_.end(); } // And finally MATLAB code is emitted, methods below called by Module::matlab_code @@ -116,6 +135,7 @@ public: /// Post-process classes for serialization markers void erase_serialization(); // non-const ! + void erase_serialization(Methods& methods); // non-const ! /// verify all of the function arguments void verifyAll(std::vector& functionNames, @@ -124,6 +144,8 @@ public: void appendInheritedMethods(const Class& cls, const std::vector& classes); + void removeInheritedNontemplateMethods(std::vector& classes); + /// The typedef line for this class, if this class is a typedef, otherwise returns an empty string. std::string getTypedef() const; @@ -141,6 +163,17 @@ public: // emit python wrapper void python_wrapper(FileWriter& wrapperFile) const; + // emit cython wrapper + void emit_cython_pxd(FileWriter& pxdFile) const; + void emit_cython_wrapper_pxd(FileWriter& pxdFile) const; + void emit_cython_pyx(FileWriter& pyxFile, + const std::vector& allClasses) const; + void pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, + const std::string& cySharedObj, + const std::vector& allClasses) const; + void pyxDynamicCast(FileWriter& pyxFile, const Class& curLevel, + const std::vector& allClasses) const; + friend std::ostream& operator<<(std::ostream& os, const Class& cls) { os << "class " << cls.name() << "{\n"; os << cls.constructor << ";\n"; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 77d831e0a..74719b289 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -24,6 +24,7 @@ #include "utilities.h" #include "Constructor.h" +#include "Class.h" using namespace std; using namespace wrap; @@ -36,20 +37,18 @@ string Constructor::matlab_wrapper_name(Str className) const { /* ************************************************************************* */ void Constructor::proxy_fragment(FileWriter& file, - const std::string& wrapperName, bool hasParent, const int id, - const ArgumentList args) const { + const std::string& wrapperName, bool hasParent, + const int id, const ArgumentList args) const { size_t nrArgs = args.size(); // check for number of arguments... file.oss << " elseif nargin == " << nrArgs; - if (nrArgs > 0) - file.oss << " && "; + if (nrArgs > 0) file.oss << " && "; // ...and their types bool first = true; for (size_t i = 0; i < nrArgs; i++) { - if (!first) - file.oss << " && "; + if (!first) file.oss << " && "; file.oss << "isa(varargin{" << i + 1 << "},'" << args[i].matlabClass(".") - << "')"; + << "')"; first = false; } // emit code for calling constructor @@ -68,26 +67,25 @@ void Constructor::proxy_fragment(FileWriter& file, /* ************************************************************************* */ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, boost::optional cppBaseClassName, int id, - const ArgumentList& al) const { - - const string wrapFunctionName = matlabUniqueName + "_constructor_" - + boost::lexical_cast(id); + Str matlabUniqueName, + boost::optional cppBaseClassName, + int id, const ArgumentList& al) const { + const string wrapFunctionName = + matlabUniqueName + "_constructor_" + boost::lexical_cast(id); file.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" - << endl; + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" + << endl; file.oss << "{\n"; file.oss << " mexAtExit(&_deleteAllObjects);\n"; - //Typedef boost::shared_ptr + // Typedef boost::shared_ptr file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;\n"; file.oss << "\n"; - //Check to see if there will be any arguments and remove {} for consiseness - if (al.size() > 0) - al.matlab_unwrap(file); // unwrap arguments + // Check to see if there will be any arguments and remove {} for consiseness + if (al.size() > 0) al.matlab_unwrap(file); // unwrap arguments file.oss << " Shared *self = new Shared(new " << cppClassName << "(" - << al.names() << "));" << endl; + << al.names() << "));" << endl; file.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; if (verbose_) @@ -96,17 +94,19 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl; file.oss << " *reinterpret_cast (mxGetData(out[0])) = self;" - << endl; + << endl; - // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) + // If we have a base class, return the base class pointer (MATLAB will call + // the base class collectorInsertAndMakeBase to add this to the collector and + // recurse the heirarchy) if (cppBaseClassName) { file.oss << "\n"; file.oss << " typedef boost::shared_ptr<" << *cppBaseClassName - << "> SharedBase;\n"; - file.oss - << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - file.oss - << " *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self);\n"; + << "> SharedBase;\n"; + file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, " + "mxREAL);\n"; + file.oss << " *reinterpret_cast(mxGetData(out[1])) = new " + "SharedBase(*self);\n"; } file.oss << "}" << endl; @@ -116,8 +116,45 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, /* ************************************************************************* */ void Constructor::python_wrapper(FileWriter& wrapperFile, Str className) const { - wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_ - << ");\n"; + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className + << "::" << name_ << ");\n"; +} + +/* ************************************************************************* */ +bool Constructor::hasDefaultConstructor() const { + for (size_t i = 0; i < nrOverloads(); i++) { + if (argumentList(i).size() == 0) return true; + } + return false; +} + +/* ************************************************************************* */ +void Constructor::emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const { + for (size_t i = 0; i < nrOverloads(); i++) { + ArgumentList args = argumentList(i); + + // generate the constructor + pxdFile.oss << " " << cls.pxdClassName() << "("; + args.emit_cython_pxd(pxdFile, cls.pxdClassName(), cls.templateArgs); + pxdFile.oss << ") " << "except +\n"; + } +} + +/* ************************************************************************* */ +void Constructor::emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const { + for (size_t i = 0; i < nrOverloads(); i++) { + ArgumentList args = argumentList(i); + pyxFile.oss << " try:\n"; + pyxFile.oss << pyx_resolveOverloadParams(args, true, 3); + pyxFile.oss + << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); + + pyxFile.oss << " self." << cls.shared_pxd_obj_in_pyx() << " = " + << cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx() + << "(" << args.pyx_asParams() << "))\n"; + pyxFile.oss << " except (AssertionError, ValueError):\n"; + pyxFile.oss << " pass\n"; + } } /* ************************************************************************* */ diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 1e061d17c..4292d5645 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -25,6 +25,9 @@ namespace wrap { +// Forward declaration +class Class; + // Constructor class struct Constructor: public OverloadedFunction { @@ -42,6 +45,9 @@ struct Constructor: public OverloadedFunction { return inst; } + /// return true if the default constructor exists + bool hasDefaultConstructor() const; + // MATLAB code generation // toolboxPath is main toolbox directory, e.g., ../matlab // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m @@ -78,6 +84,10 @@ struct Constructor: public OverloadedFunction { // emit python wrapper void python_wrapper(FileWriter& wrapperFile, Str className) const; + // emit cython wrapper + void emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const; + void emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const; + friend std::ostream& operator<<(std::ostream& os, const Constructor& m) { for (size_t i = 0; i < m.nrOverloads(); i++) os << m.name_ << m.argLists_[i]; diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 783661819..2c3843b37 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (CantOpenFile) { + } catch (const CantOpenFile& e) { file_exists = false; } diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 5ec022ca4..127823e82 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -23,11 +23,14 @@ namespace wrap { + class Class; + struct ForwardDeclaration { - std::string name; + Class cls; bool isVirtual; ForwardDeclaration() : isVirtual(false) {} - ForwardDeclaration(const std::string& s) : name(s), isVirtual(false) {} + explicit ForwardDeclaration(const std::string& s) : cls(s), isVirtual(false) {} + std::string name() const { return cls.qualifiedName("::"); } }; } diff --git a/wrap/FullyOverloadedFunction.cpp b/wrap/FullyOverloadedFunction.cpp new file mode 100644 index 000000000..4db4c8713 --- /dev/null +++ b/wrap/FullyOverloadedFunction.cpp @@ -0,0 +1,34 @@ +#include "FullyOverloadedFunction.h" + +using namespace std; + +namespace wrap { +const std::array FullyOverloadedFunction::pythonKeywords{ + {"print", "lambda"}}; + +/* ************************************************************************* */ +std::string FullyOverloadedFunction::pyx_functionCall( + const std::string& caller, + const std::string& funcName, size_t iOverload) const { + + string ret; + if (!returnVals_[iOverload].isPair && !returnVals_[iOverload].type1.isPtr && + returnVals_[iOverload].type1.isNonBasicType()) { + ret = returnVals_[iOverload].type1.make_shared_pxd_class_in_pyx() + "("; + } + + // actual function call ... + if (!caller.empty()) ret += caller + "."; + ret += funcName; + if (templateArgValue_) ret += "[" + templateArgValue_->pxd_class_in_pyx() + "]"; + //... with argument list + ret += "(" + argumentList(iOverload).pyx_asParams() + ")"; + + if (!returnVals_[iOverload].isPair && !returnVals_[iOverload].type1.isPtr && + returnVals_[iOverload].type1.isNonBasicType()) + ret += ")"; + + return ret; +} + +} diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index 80d974d88..87c5169dd 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -19,6 +19,7 @@ #pragma once #include "OverloadedFunction.h" +#include namespace wrap { @@ -27,7 +28,7 @@ namespace wrap { */ class SignatureOverloads: public ArgumentOverloads { -protected: +public: std::vector returnVals_; @@ -116,6 +117,19 @@ public: return first; } + // emit cython pyx function call + std::string pyx_functionCall(const std::string& caller, const std::string& funcName, + size_t iOverload) const; + + /// Cython: Rename functions which names are python keywords + static const std::array pythonKeywords; + static std::string pyRename(const std::string& name) { + if (std::find(pythonKeywords.begin(), pythonKeywords.end(), name) == + pythonKeywords.end()) + return name; + else + return name + "_"; + } }; // Templated checking functions diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 3f667e2c9..c8482f2c4 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -6,6 +6,7 @@ */ #include "GlobalFunction.h" +#include "Class.h" #include "utilities.h" #include @@ -16,11 +17,12 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(const Qualified& overload, - const ArgumentList& args, const ReturnValue& retVal, + const ArgumentList& args, const ReturnValue& retVal, const std::string& _includeFile, boost::optional instName, bool verbose) { FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName, verbose); overloads.push_back(overload); + includeFile = _includeFile; } /* ************************************************************************* */ @@ -131,6 +133,93 @@ void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const { wrapperFile.oss << "def(\"" << name_ << "\", " << name_ << ");\n"; } +/* ************************************************************************* */ +void GlobalFunction::emit_cython_pxd(FileWriter& file) const { + file.oss << "cdef extern from \"" << includeFile << "\" namespace \"" + << overloads[0].qualifiedNamespaces("::") + << "\":" << endl; + for (size_t i = 0; i < nrOverloads(); ++i) { + file.oss << " "; + returnVals_[i].emit_cython_pxd(file, "", vector()); + file.oss << pxdName() + " \"" + overloads[0].qualifiedName("::") + + "\"("; + argumentList(i).emit_cython_pxd(file, "", vector()); + file.oss << ")"; + file.oss << "\n"; + } +} + +/* ************************************************************************* */ +void GlobalFunction::emit_cython_pyx_no_overload(FileWriter& file) const { + string funcName = pyxName(); + + // Function definition + file.oss << "def " << funcName; + + // modify name of function instantiation as python doesn't allow overloads + // e.g. template funcName(...) --> funcNameA, funcNameB, funcNameC + if (templateArgValue_) file.oss << templateArgValue_->pyxClassName(); + + // funtion arguments + file.oss << "("; + argumentList(0).emit_cython_pyx(file); + file.oss << "):\n"; + + /// Call cython corresponding function and return + file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder(" "); + string ret = pyx_functionCall("", pxdName(), 0); + if (!returnVals_[0].isVoid()) { + file.oss << " cdef " << returnVals_[0].pyx_returnType() + << " ret = " << ret << "\n"; + file.oss << " return " << returnVals_[0].pyx_casting("ret") << "\n"; + } else { + file.oss << " " << ret << "\n"; + } +} + +/* ************************************************************************* */ +void GlobalFunction::emit_cython_pyx(FileWriter& file) const { + string funcName = pyxName(); + + size_t N = nrOverloads(); + if (N == 1) { + emit_cython_pyx_no_overload(file); + return; + } + + // Dealing with overloads.. + file.oss << "def " << funcName << "(*args, **kwargs):\n"; + for (size_t i = 0; i < N; ++i) { + file.oss << " success, results = " << funcName << "_" << i + << "(args, kwargs)\n"; + file.oss << " if success:\n return results\n"; + } + file.oss << " raise TypeError('Could not find the correct overload')\n"; + + for (size_t i = 0; i < N; ++i) { + ArgumentList args = argumentList(i); + file.oss << "def " + funcName + "_" + to_string(i) + "(args, kwargs):\n"; + file.oss << " cdef list __params\n"; + if (!returnVals_[i].isVoid()) { + file.oss << " cdef " << returnVals_[i].pyx_returnType() << " return_value\n"; + } + file.oss << " try:\n"; + file.oss << pyx_resolveOverloadParams(args, false, 2); // lazy: always return None even if it's a void function + + /// Call corresponding cython function + file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); + string call = pyx_functionCall("", pxdName(), i); + if (!returnVals_[i].isVoid()) { + file.oss << " return_value = " << call << "\n"; + file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n"; + } else { + file.oss << " " << call << "\n"; + file.oss << " return True, None\n"; + } + file.oss << " except:\n"; + file.oss << " return False, None\n\n"; + } +} /* ************************************************************************* */ } // \namespace wrap diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index b2a582654..473ebadef 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -28,10 +28,11 @@ namespace wrap { struct GlobalFunction: public FullyOverloadedFunction { std::vector overloads; ///< Stack of qualified names + std::string includeFile; // adds an overloaded version of this function, void addOverload(const Qualified& overload, const ArgumentList& args, - const ReturnValue& retVal, boost::optional instName = + const ReturnValue& retVal, const std::string& _includeFile = "", boost::optional instName = boost::none, bool verbose = false); void verifyArguments(const std::vector& validArgs) const { @@ -50,6 +51,16 @@ struct GlobalFunction: public FullyOverloadedFunction { // emit python wrapper void python_wrapper(FileWriter& wrapperFile) const; + // 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_); } + + // emit cython wrapper + void emit_cython_pxd(FileWriter& pxdFile) const; + void emit_cython_pyx(FileWriter& pyxFile) const; + void emit_cython_pyx_no_overload(FileWriter& pyxFile) const; + private: // Creates a single global function - all in same namespace @@ -67,12 +78,15 @@ struct GlobalFunctionGrammar: public classic::grammar { GlobalFunctions& global_functions_; ///< successful parse will be placed in here std::vector& namespaces_; + std::string& includeFile; /// Construct type grammar and specify where result is placed GlobalFunctionGrammar(GlobalFunctions& global_functions, - std::vector& namespaces) : - global_functions_(global_functions), namespaces_(namespaces) { - } + std::vector& namespaces, + std::string& includeFile) + : global_functions_(global_functions), + namespaces_(namespaces), + includeFile(includeFile) {} /// Definition of type grammar template @@ -101,16 +115,16 @@ struct GlobalFunctionGrammar: public classic::grammar { globalFunctionName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; // parse a global function - global_function_p = (returnValue_g - >> globalFunctionName_p[assign_a(globalFunction.name_)] - >> argumentList_g >> ';' >> *comments_p) // - [assign_a(globalFunction.namespaces_, self.namespaces_)][bl::bind( + global_function_p = (returnValue_g >> globalFunctionName_p[assign_a( + globalFunction.name_)] >> + argumentList_g >> ';' >> *comments_p) // + [assign_a(globalFunction.namespaces_, self.namespaces_)] // + [bl::bind( &GlobalFunction::addOverload, bl::var(self.global_functions_)[bl::var(globalFunction.name_)], - bl::var(globalFunction), bl::var(args), bl::var(retVal), - boost::none, verbose)] // + bl::var(globalFunction), bl::var(args), bl::var(retVal), bl::var(self.includeFile), + boost::none, verbose)] // [assign_a(retVal, retVal0)][clear_a(globalFunction)][clear_a(args)]; - } classic::rule const& start() const { diff --git a/wrap/Method.cpp b/wrap/Method.cpp index f8c03b0c6..1a58692b6 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -16,6 +16,7 @@ **/ #include "Method.h" +#include "Class.h" #include "utilities.h" #include @@ -29,39 +30,43 @@ using namespace wrap; /* ************************************************************************* */ bool Method::addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, - boost::optional instName, bool verbose) { + const ReturnValue& retVal, bool is_const, + boost::optional instName, + bool verbose) { bool first = MethodBase::addOverload(name, args, retVal, instName, verbose); if (first) is_const_ = is_const; else if (is_const && !is_const_) throw std::runtime_error( - "Method::addOverload now designated as const whereas before it was not"); + "Method::addOverload now designated as const whereas before it was " + "not"); else if (!is_const && is_const_) throw std::runtime_error( - "Method::addOverload now designated as non-const whereas before it was"); + "Method::addOverload now designated as non-const whereas before it " + "was"); return first; } /* ************************************************************************* */ void Method::proxy_header(FileWriter& proxyFile) const { proxyFile.oss << " function varargout = " << matlabName() - << "(this, varargin)\n"; + << "(this, varargin)\n"; } /* ************************************************************************* */ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args) const { + Str matlabUniqueName, + const ArgumentList& args) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); wrapperFile.oss << " checkArguments(\"" << matlabName() - << "\",nargout,nargin-1," << args.size() << ");\n"; + << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName - << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp, we start at 1 as first is obj args.matlab_unwrap(wrapperFile, 1); @@ -76,3 +81,125 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, } /* ************************************************************************* */ +void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const { + for (size_t i = 0; i < nrOverloads(); ++i) { + file.oss << " "; + returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs); + const string renamed = pyRename(name_); + if (renamed != name_) { + file.oss << pyRename(name_) + " \"" + name_ + "\"" << "("; + } else { + file.oss << name_ << "("; + } + argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs); + file.oss << ")"; + // if (is_const_) file.oss << " const"; + file.oss << " except +"; + file.oss << "\n"; + } +} + +/* ************************************************************************* */ +void Method::emit_cython_pyx_no_overload(FileWriter& file, + const Class& cls) const { + string funcName = pyRename(name_); + + // leverage python's special treatment for print + if (funcName == "print_") { + file.oss << " def __repr__(self):\n"; + file.oss << " strBuf = RedirectCout()\n"; + file.oss << " self.print_('')\n"; + file.oss << " return strBuf.str()\n"; + } + + // Function definition + file.oss << " def " << funcName; + + // modify name of function instantiation as python doesn't allow overloads + // e.g. template funcName(...) --> funcNameA, funcNameB, funcNameC + if (templateArgValue_) file.oss << templateArgValue_->pyxClassName(); + + // function arguments + file.oss << "(self"; + if (argumentList(0).size() > 0) file.oss << ", "; + argumentList(0).emit_cython_pyx(file); + file.oss << "):\n"; + + /// Call cython corresponding function and return + file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder(" "); + string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()"; + string ret = pyx_functionCall(caller, funcName, 0); + if (!returnVals_[0].isVoid()) { + file.oss << " cdef " << returnVals_[0].pyx_returnType() + << " ret = " << ret << "\n"; + file.oss << " return " << returnVals_[0].pyx_casting("ret") << "\n"; + } else { + file.oss << " " << ret << "\n"; + } +} + +/* ************************************************************************* */ +void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const { + string funcName = pyRename(name_); + // For template function: modify name of function instantiation as python + // doesn't allow overloads + // e.g. template funcName(...) --> funcNameA, funcNameB, funcNameC + string instantiatedName = + (templateArgValue_) ? funcName + templateArgValue_->pyxClassName() : + funcName; + + size_t N = nrOverloads(); + // It's easy if there's no overload + if (N == 1) { + emit_cython_pyx_no_overload(file, cls); + return; + } + + // Dealing with overloads.. + file.oss << " def " << instantiatedName << "(self, *args, **kwargs):\n"; + file.oss << " cdef list __params\n"; + + // Define return values for all possible overloads + vector return_type; // every overload has a return type, possibly void + map return_value; // we only define one return value for every distinct type + size_t j = 1; + for (size_t i = 0; i < nrOverloads(); ++i) { + if (returnVals_[i].isVoid()) { + return_type.push_back("void"); + } else { + const string type = returnVals_[i].pyx_returnType(); + return_type.push_back(type); + if (return_value.count(type) == 0) { + const string value = "return_value_" + to_string(j++); + return_value[type] = value; + file.oss << " cdef " << type << " " << value << "\n"; + } + } + } + + for (size_t i = 0; i < nrOverloads(); ++i) { + ArgumentList args = argumentList(i); + file.oss << " try:\n"; + file.oss << pyx_resolveOverloadParams(args, false, 3); // lazy: always return None even if it's a void function + + /// Call corresponding cython function + file.oss << args.pyx_convertEigenTypeAndStorageOrder(" "); + string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()"; + string call = pyx_functionCall(caller, funcName, i); + if (!returnVals_[i].isVoid()) { + const string type = return_type[i]; + const string value = return_value[type]; + file.oss << " " << value << " = " << call << "\n"; + file.oss << " return " << returnVals_[i].pyx_casting(value) + << "\n"; + } else { + file.oss << " " << call << "\n"; + file.oss << " return\n"; + } + file.oss << " except (AssertionError, ValueError):\n"; + file.oss << " pass\n"; + } + file.oss + << " raise TypeError('Incorrect arguments for method call.')\n\n"; +} +/* ************************************************************************* */ diff --git a/wrap/Method.h b/wrap/Method.h index 33ff7072e..bfa4a65da 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -25,6 +25,7 @@ namespace wrap { /// Method class class Method: public MethodBase { +protected: bool is_const_; public: @@ -44,12 +45,22 @@ public: return is_const_; } + bool isSameModifiers(const Method& other) const { + return is_const_ == other.is_const_ && + ((templateArgValue_ && other.templateArgValue_) || + (!templateArgValue_ && !other.templateArgValue_)); + } + friend std::ostream& operator<<(std::ostream& os, const Method& m) { for (size_t i = 0; i < m.nrOverloads(); i++) os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; return os; } + void emit_cython_pxd(FileWriter& file, const Class& cls) const; + void emit_cython_pyx(FileWriter& file, const Class& cls) const; + void emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const; + private: // Emit method header diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp index 84e6c67d9..ef169d989 100644 --- a/wrap/MethodBase.cpp +++ b/wrap/MethodBase.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -17,6 +17,7 @@ **/ #include "Method.h" +#include "Class.h" #include "utilities.h" #include @@ -29,12 +30,11 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, - Str matlabUniqueName, Str wrapperName, +void MethodBase::proxy_wrapper_fragments( + FileWriter& proxyFile, FileWriter& wrapperFile, Str cppClassName, + Str matlabQualName, Str matlabUniqueName, Str wrapperName, const TypeAttributesTable& typeAttributes, vector& functionNames) const { - // emit header, e.g., function varargout = templatedMethod(this, varargin) proxy_header(proxyFile); @@ -45,36 +45,36 @@ void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, // Emit URL to Doxygen page proxyFile.oss << " % " - << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" - << endl; + << "Doxygen can be found at " + "http://research.cc.gatech.edu/borg/sites/edu.borg/html/" + "index.html" << endl; // Handle special case of single overload with all numeric arguments if (nrOverloads() == 1 && argumentList(0).allScalar()) { // Output proxy matlab code // TODO: document why is it OK to not check arguments in this case proxyFile.oss << " "; - const int id = (int) functionNames.size(); + const int id = (int)functionNames.size(); emit_call(proxyFile, returnValue(0), wrapperName, id); // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, 0, id, typeAttributes); + const string wrapFunctionName = wrapper_fragment( + wrapperFile, cppClassName, matlabUniqueName, 0, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); } else { // Check arguments for all overloads for (size_t i = 0; i < nrOverloads(); ++i) { - // Output proxy matlab code proxyFile.oss << " " << (i == 0 ? "" : "else"); - const int id = (int) functionNames.size(); + const int id = (int)functionNames.size(); emit_conditional_call(proxyFile, returnValue(i), argumentList(i), - wrapperName, id); + wrapperName, id); // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, - cppClassName, matlabUniqueName, i, id, typeAttributes); + const string wrapFunctionName = wrapper_fragment( + wrapperFile, cppClassName, matlabUniqueName, i, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); @@ -90,20 +90,20 @@ void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, } /* ************************************************************************* */ -string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes) const { - +string MethodBase::wrapper_fragment( + FileWriter& wrapperFile, Str cppClassName, Str matlabUniqueName, + int overload, int id, const TypeAttributesTable& typeAttributes) const { // generate code - const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" - + boost::lexical_cast(id); + const string wrapFunctionName = + matlabUniqueName + "_" + name_ + "_" + boost::lexical_cast(id); const ArgumentList& args = argumentList(overload); const ReturnValue& returnVal = returnValue(overload); // call - wrapperFile.oss << "void " << wrapFunctionName + wrapperFile.oss + << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start wrapperFile.oss << "{\n"; @@ -111,13 +111,13 @@ string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, returnVal.wrapTypeUnwrap(wrapperFile); wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; + << "> Shared;" << endl; // get call // for static methods: cppClassName::staticMethod // for instance methods: obj->instanceMethod - string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, - args); + string expanded = + wrapper_call(wrapperFile, cppClassName, matlabUniqueName, args); expanded += ("(" + args.names() + ")"); if (returnVal.type1.name() != "void") @@ -133,8 +133,8 @@ string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, /* ************************************************************************* */ void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const { - wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" - << name_ << ");\n"; + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className + << "::" << name_ << ");\n"; } /* ************************************************************************* */ diff --git a/wrap/MethodBase.h b/wrap/MethodBase.h index 903b89569..ee72a6a53 100644 --- a/wrap/MethodBase.h +++ b/wrap/MethodBase.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -23,9 +23,11 @@ namespace wrap { -/// MethodBase class -struct MethodBase: public FullyOverloadedFunction { +// Forward declaration +class Class; +/// MethodBase class +struct MethodBase : public FullyOverloadedFunction { typedef const std::string& Str; // emit a list of comments, one for each overload @@ -44,24 +46,25 @@ struct MethodBase: public FullyOverloadedFunction { // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - Str cppClassName, Str matlabQualName, Str matlabUniqueName, - Str wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + Str cppClassName, Str matlabQualName, + Str matlabUniqueName, Str wrapperName, + const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; // emit python wrapper void python_wrapper(FileWriter& wrapperFile, Str className) const; protected: - virtual void proxy_header(FileWriter& proxyFile) const = 0; - std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + std::string wrapper_fragment( + FileWriter& wrapperFile, Str cppClassName, Str matlabUniqueName, + int overload, int id, + const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args) const = 0; + Str matlabUniqueName, + const ArgumentList& args) const = 0; }; -} // \namespace wrap - +} // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 61d2a29e0..9eee686cb 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -1,52 +1,54 @@ -/* ---------------------------------------------------------------------------- - - * 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 Module.ccp - * @author Frank Dellaert - * @author Alex Cunningham - * @author Andrew Melim +/* ---------------------------------------------------------------------------- + + * 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 Module.ccp + * @author Frank Dellaert + * @author Alex Cunningham + * @author Andrew Melim * @author Richard Roberts - **/ - -#include "Module.h" -#include "FileWriter.h" -#include "TypeAttributesTable.h" + **/ + +#include "Module.h" +#include "FileWriter.h" +#include "TypeAttributesTable.h" #include "utilities.h" -#include -#include - -#include -#include - -using namespace std; -using namespace wrap; -using namespace BOOST_SPIRIT_CLASSIC_NS; -namespace bl = boost::lambda; -namespace fs = boost::filesystem; - -/* ************************************************************************* */ -// We parse an interface file into a Module object. -// The grammar is defined using the boost/spirit combinatorial parser. -// For example, str_p("const") parses the string "const", and the >> -// operator creates a sequence parser. The grammar below, composed of rules -// and with start rule [class_p], doubles as the specs for our interface files. -/* ************************************************************************* */ - -/* ************************************************************************* */ +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; +using namespace BOOST_SPIRIT_CLASSIC_NS; +namespace bl = boost::lambda; +namespace fs = boost::filesystem; + +/* ************************************************************************* */ +// We parse an interface file into a Module object. +// The grammar is defined using the boost/spirit combinatorial parser. +// For example, str_p("const") parses the string "const", and the >> +// operator creates a sequence parser. The grammar below, composed of rules +// and with start rule [class_p], doubles as the specs for our interface files. +/* ************************************************************************* */ + +/* ************************************************************************* */ // If a number of template arguments were given, generate a number of expanded // class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes -static void handle_possible_template(vector& classes, const Class& cls, - const Template& t) { +static void handle_possible_template(vector& classes, + vector& uninstantiatedClasses, + const Class& cls, const Template& t) { + uninstantiatedClasses.push_back(cls); if (cls.templateArgs.empty() || t.empty()) { classes.push_back(cls); } else { @@ -62,17 +64,24 @@ static void handle_possible_template(vector& classes, const Class& cls, } } +static void push_typedef_pair(vector& typedefs, + const Qualified& oldType, + const Qualified& newType, + const string& includeFile) { + typedefs.push_back(TypedefPair(oldType, newType, includeFile)); +} + /* ************************************************************************* */ Module::Module(const std::string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) { } -/* ************************************************************************* */ -Module::Module(const string& interfacePath, +/* ************************************************************************* */ +Module::Module(const string& interfacePath, const string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) -{ +{ // read interface file string interfaceFile = interfacePath + "/" + moduleName + ".h"; string contents = file_contents(interfaceFile); @@ -84,53 +93,69 @@ Module::Module(const string& interfacePath, /* ************************************************************************* */ void Module::parseMarkup(const std::string& data) { // The parse imperatively :-( updates variables gradually during parse - // The one with postfix 0 are used to reset the variables after parse. - - //---------------------------------------------------------------------------- - // Grammar with actions that build the Class object. Actions are - // defined within the square brackets [] and are executed whenever a - // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug. - // The grammar is allows a very restricted C++ header - // lexeme_d turns off white space skipping - // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html - // ---------------------------------------------------------------------------- - + // The one with postfix 0 are used to reset the variables after parse. + + //---------------------------------------------------------------------------- + // Grammar with actions that build the Class object. Actions are + // defined within the square brackets [] and are executed whenever a + // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug. + // The grammar is allows a very restricted C++ header + // lexeme_d turns off white space skipping + // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html + // ---------------------------------------------------------------------------- + // Define Rule and instantiate basic rules typedef rule Rule; BasicRules basic; vector namespaces; // current namespace tag + string currentInclude; // parse a full class Class cls0(verbose),cls(verbose); Template classTemplate; ClassGrammar class_g(cls,classTemplate); - Rule class_p = class_g // + Rule class_p = class_g // [assign_a(cls.namespaces_, namespaces)] - [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), - bl::var(classTemplate))] - [clear_a(classTemplate)] // - [assign_a(cls,cls0)]; + [assign_a(cls.includeFile, currentInclude)][bl::bind( + &handle_possible_template, bl::var(classes), + bl::var(uninstantiatedClasses), bl::var(cls), + bl::var(classTemplate))][clear_a(classTemplate)] // + [assign_a(cls, cls0)]; // parse "gtsam::Pose2" and add to singleInstantiation.typeList TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); - + // typedef gtsam::RangeFactor RangeFactorPosePoint2; TypeGrammar instantiationClass_g(singleInstantiation.class_); - Rule templateSingleInstantiation_p = + Rule templateSingleInstantiation_p = (str_p("typedef") >> instantiationClass_g >> typelist_g >> basic.className_p[assign_a(singleInstantiation.name_)] >> - ';') + ';') [assign_a(singleInstantiation.namespaces_, namespaces)] - [push_back_a(templateInstantiationTypedefs, singleInstantiation)] - [assign_a(singleInstantiation, singleInstantiation0)]; - + [push_back_a(templateInstantiationTypedefs, singleInstantiation)] + [assign_a(singleInstantiation, singleInstantiation0)]; + + Qualified oldType, newType; + TypeGrammar typedefOldClass_g(oldType), typedefNewClass_g(newType); + Rule typedef_p = + (str_p("typedef") >> typedefOldClass_g >> typedefNewClass_g >> + ';') + [assign_a(oldType.namespaces_, namespaces)] + [assign_a(newType.namespaces_, namespaces)] + [bl::bind(&push_typedef_pair, bl::var(typedefs), bl::var(oldType), + bl::var(newType), bl::var(currentInclude))]; + // Create grammar for global functions - GlobalFunctionGrammar global_function_g(global_functions,namespaces); - - Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); + GlobalFunctionGrammar global_function_g(global_functions, namespaces, + currentInclude); + + Rule include_p = str_p("#include") >> ch_p('<') >> + (*(anychar_p - '>'))[push_back_a(includes)] + [assign_a(currentInclude)] >> + ch_p('>'); #ifdef __clang__ #pragma clang diagnostic push @@ -141,7 +166,7 @@ void Module::parseMarkup(const std::string& data) { (str_p("namespace") >> basic.namespace_p[push_back_a(namespaces)] >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_g | namespace_def_p | basic.comments_p) + >> *(include_p | class_p | templateSingleInstantiation_p | typedef_p | global_function_g | namespace_def_p | basic.comments_p) >> ch_p('}')) [pop_a(namespaces)]; @@ -151,21 +176,27 @@ void Module::parseMarkup(const std::string& data) { // parse forward declaration ForwardDeclaration fwDec0, fwDec; + Class fwParentClass; + TypeGrammar className_g(fwDec.cls); + TypeGrammar classParent_g(fwParentClass); + Rule classParent_p = (':' >> classParent_g >> ';') // + [bl::bind(&Class::assignParent, bl::var(fwDec.cls), + bl::var(fwParentClass))][clear_a(fwParentClass)]; + Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, T)]) - >> str_p("class") - >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] - >> ch_p(';') - [push_back_a(forward_declarations, fwDec)] + >> str_p("class") >> className_g + >> (classParent_p | ';') + [push_back_a(forward_declarations, fwDec)] [assign_a(cls,cls0)] // also clear class to avoid partial parse - [assign_a(fwDec, fwDec0)]; - + [assign_a(fwDec, fwDec0)]; + Rule module_content_p = basic.comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p | global_function_g | namespace_def_p; - - Rule module_p = *module_content_p >> !end_p; - + + Rule module_p = *module_content_p >> !end_p; + // and parse contents parse_info info = parse(data.c_str(), module_p, space_p); if(!info.full) { @@ -179,10 +210,23 @@ void Module::parseMarkup(const std::string& data) { for(Class& cls: classes) cls.erase_serialization(); + for(Class& cls: uninstantiatedClasses) + cls.erase_serialization(); + // Explicitly add methods to the classes from parents so it shows in documentation for(Class& cls: classes) cls.appendInheritedMethods(cls, classes); + // - Remove inherited methods for Cython classes in the pxd, otherwise Cython can't decide which one to call. + // - Only inherited nontemplateMethods_ in uninstantiatedClasses need to be removed + // because that what we serialized to the pxd. + // - However, we check against the class parent's *methods_* to avoid looking into + // its grand parent and grand-grand parent, etc., because all those are already + // added in its direct parent. + // - So this must be called *after* the above code appendInheritedMethods!! + for(Class& cls: uninstantiatedClasses) + cls.removeInheritedNontemplateMethods(uninstantiatedClasses); + // Expand templates - This is done first so that template instantiations are // counted in the list of valid types, have their attributes and dependencies // checked, etc. @@ -191,7 +235,7 @@ void Module::parseMarkup(const std::string& data) { // Dependency check list vector validTypes = GenerateValidTypes(expandedClasses, - forward_declarations); + forward_declarations, typedefs); // Check that all classes have been defined somewhere verifyArguments(validTypes, global_functions); @@ -204,16 +248,18 @@ void Module::parseMarkup(const std::string& data) { // Create type attributes table and check validity typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); + for (const TypedefPair p: typedefs) + typeAttributes.addType(p.newType); // add Eigen types as template arguments are also checked ? vector eigen; eigen.push_back(ForwardDeclaration("Vector")); eigen.push_back(ForwardDeclaration("Matrix")); typeAttributes.addForwardDeclarations(eigen); typeAttributes.checkValidity(expandedClasses); -} - -/* ************************************************************************* */ -void Module::matlab_code(const string& toolboxPath) const { +} + +/* ************************************************************************* */ +void Module::generate_matlab_wrapper(const string& toolboxPath) const { fs::create_directories(toolboxPath); @@ -273,7 +319,127 @@ void Module::matlab_code(const string& toolboxPath) const { wrapperFile.emit(true); } -/* ************************************************************************* */ +/* ************************************************************************* */ +void Module::generate_cython_wrapper(const string& toolboxPath, const std::string& pxdImports) const { + fs::create_directories(toolboxPath); + string pxdFileName = toolboxPath + "/" + name + ".pxd"; + FileWriter pxdFile(pxdFileName, verbose, "#"); + pxdFile.oss << pxdImports << "\n"; + emit_cython_pxd(pxdFile); + string pyxFileName = toolboxPath + "/" + name + ".pyx"; + FileWriter pyxFile(pyxFileName, verbose, "#"); + emit_cython_pyx(pyxFile); +} + +/* ************************************************************************* */ +void Module::emit_cython_pxd(FileWriter& pxdFile) const { + // headers + pxdFile.oss << "from gtsam_eigency.core cimport *\n" + "from libcpp.string cimport string\n" + "from libcpp.vector cimport vector\n" + "from libcpp.pair cimport pair\n" + "from libcpp.set cimport set\n" + "from libcpp.map cimport map\n" + "from libcpp cimport bool\n\n"; + + // boost shared_ptr + pxdFile.oss << "cdef extern from \"boost/shared_ptr.hpp\" namespace \"boost\":\n" + " cppclass shared_ptr[T]:\n" + " shared_ptr()\n" + " shared_ptr(T*)\n" + " 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] make_shared[T](const T& r)\n\n"; + + for(const TypedefPair& types: typedefs) + types.emit_cython_pxd(pxdFile); + + //... wrap all classes + for (const Class& cls : uninstantiatedClasses) { + cls.emit_cython_pxd(pxdFile); + + for (const Class& expCls : expandedClasses) { + bool matchingNonTemplated = !expCls.templateClass + && expCls.pxdClassName() == cls.pxdClassName(); + bool isTemplatedFromCls = expCls.templateClass + && expCls.templateClass->pxdClassName() == cls.pxdClassName(); + + // ctypedef for template instantiations + if (isTemplatedFromCls) { + pxdFile.oss << "\n"; + pxdFile.oss << "ctypedef " << expCls.templateClass->pxdClassName() + << "["; + for (size_t i = 0; i < expCls.templateInstTypeList.size(); ++i) + pxdFile.oss << expCls.templateInstTypeList[i].pxdClassName() + << ((i == expCls.templateInstTypeList.size() - 1) ? "" : ", "); + pxdFile.oss << "] " << expCls.pxdClassName() << "\n"; + } + + // Python wrapper class + if (isTemplatedFromCls || matchingNonTemplated) { + expCls.emit_cython_wrapper_pxd(pxdFile); + } + } + pxdFile.oss << "\n\n"; + } + + //... wrap global functions + for(const GlobalFunctions::value_type& p: global_functions) + p.second.emit_cython_pxd(pxdFile); + + pxdFile.emit(true); +} + +/* ************************************************************************* */ +void Module::emit_cython_pyx(FileWriter& pyxFile) const { + // headers... + string pxdHeader = name; + pyxFile.oss << "cimport numpy as np\n" + "import numpy as npp\n" + "cimport " << pxdHeader << "\n" + "from "<< pxdHeader << " cimport shared_ptr\n" + "from "<< pxdHeader << " cimport dynamic_pointer_cast\n" + "from "<< pxdHeader << " cimport make_shared\n"; + + pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n" + "cdef list process_args(list keywords, tuple args, dict kwargs):\n" + " cdef str keyword\n" + " cdef int n = len(args), m = len(keywords)\n" + " cdef list params = list(args)\n" + " assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m)\n" + " try:\n" + " return params + [kwargs[keyword] for keyword in keywords[n:]]\n" + " except:\n" + " raise ValueError('Epected arguments ' + str(keywords))\n"; + + // import all typedefs, e.g. from gtsam_wrapper cimport Key, so we don't need to say gtsam.Key + for(const Qualified& q: Qualified::BasicTypedefs) { + pyxFile.oss << "from " << pxdHeader << " cimport " << q.pxdClassName() << "\n"; + } + pyxFile.oss << "from gtsam_eigency.core cimport *\n" + "from libcpp cimport bool\n\n" + "from libcpp.pair cimport pair\n" + "from libcpp.string cimport string\n" + "from cython.operator cimport dereference as deref\n\n\n"; + + // all classes include all forward declarations + std::vector allClasses = expandedClasses; + for(const ForwardDeclaration& fd: forward_declarations) + allClasses.push_back(fd.cls); + + for(const Class& cls: expandedClasses) + cls.emit_cython_pyx(pyxFile, allClasses); + pyxFile.oss << "\n"; + + //... wrap global functions + for(const GlobalFunctions::value_type& p: global_functions) + p.second.emit_cython_pyx(pyxFile); + pyxFile.emit(true); +} + +/* ************************************************************************* */ void Module::generateIncludes(FileWriter& file) const { // collect includes @@ -291,158 +457,161 @@ void Module::generateIncludes(FileWriter& file) const { /* ************************************************************************* */ - void Module::finish_wrapper(FileWriter& file, const std::vector& functionNames) const { - file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - file.oss << "{\n"; - file.oss << " mstream mout;\n"; // Send stdout to MATLAB console - file.oss << " std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n"; - file.oss << " _" << name << "_RTTIRegister();\n\n"; - file.oss << " int id = unwrap(in[0]);\n\n"; - file.oss << " try {\n"; - file.oss << " switch(id) {\n"; - for(size_t id = 0; id < functionNames.size(); ++id) { - file.oss << " case " << id << ":\n"; - file.oss << " " << functionNames[id] << "(nargout, out, nargin-1, in+1);\n"; - file.oss << " break;\n"; - } - file.oss << " }\n"; - file.oss << " } catch(const std::exception& e) {\n"; - file.oss << " mexErrMsgTxt((\"Exception from gtsam:\\n\" + std::string(e.what()) + \"\\n\").c_str());\n"; - file.oss << " }\n"; - file.oss << "\n"; - file.oss << " std::cout.rdbuf(outbuf);\n"; // Restore cout - file.oss << "}\n"; - } - -/* ************************************************************************* */ -vector Module::ExpandTypedefInstantiations(const vector& classes, const vector instantiations) { - - vector expandedClasses = classes; - + void Module::finish_wrapper(FileWriter& file, const std::vector& functionNames) const { + file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "{\n"; + file.oss << " mstream mout;\n"; // Send stdout to MATLAB console + file.oss << " std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n"; + file.oss << " _" << name << "_RTTIRegister();\n\n"; + file.oss << " int id = unwrap(in[0]);\n\n"; + file.oss << " try {\n"; + file.oss << " switch(id) {\n"; + for(size_t id = 0; id < functionNames.size(); ++id) { + file.oss << " case " << id << ":\n"; + file.oss << " " << functionNames[id] << "(nargout, out, nargin-1, in+1);\n"; + file.oss << " break;\n"; + } + file.oss << " }\n"; + file.oss << " } catch(const std::exception& e) {\n"; + file.oss << " mexErrMsgTxt((\"Exception from gtsam:\\n\" + std::string(e.what()) + \"\\n\").c_str());\n"; + file.oss << " }\n"; + file.oss << "\n"; + file.oss << " std::cout.rdbuf(outbuf);\n"; // Restore cout + file.oss << "}\n"; + } + +/* ************************************************************************* */ +vector Module::ExpandTypedefInstantiations(const vector& classes, const vector instantiations) { + + vector expandedClasses = classes; + for(const TemplateInstantiationTypedef& inst: instantiations) { - // Add the new class to the list - expandedClasses.push_back(inst.findAndExpand(classes)); - } - - // Remove all template classes - for(size_t i = 0; i < expandedClasses.size(); ++i) - if(!expandedClasses[size_t(i)].templateArgs.empty()) { - expandedClasses.erase(expandedClasses.begin() + size_t(i)); - -- i; - } - - return expandedClasses; -} - -/* ************************************************************************* */ -vector Module::GenerateValidTypes(const vector& classes, const vector forwardDeclarations) { - vector validTypes; + // Add the new class to the list + expandedClasses.push_back(inst.findAndExpand(classes)); + } + + // Remove all template classes + for(size_t i = 0; i < expandedClasses.size(); ++i) + if(!expandedClasses[i].templateArgs.empty()) { + expandedClasses.erase(expandedClasses.begin() + size_t(i)); + -- i; + } + + return expandedClasses; +} + +/* ************************************************************************* */ +vector Module::GenerateValidTypes(const vector& classes, const vector& forwardDeclarations, const vector& typedefs) { + vector validTypes; for(const ForwardDeclaration& fwDec: forwardDeclarations) { - validTypes.push_back(fwDec.name); - } - validTypes.push_back("void"); - validTypes.push_back("string"); - validTypes.push_back("int"); - validTypes.push_back("bool"); - validTypes.push_back("char"); - validTypes.push_back("unsigned char"); - validTypes.push_back("size_t"); - validTypes.push_back("double"); - validTypes.push_back("Vector"); - validTypes.push_back("Matrix"); - //Create a list of parsed classes for dependency checking + validTypes.push_back(fwDec.name()); + } + validTypes.push_back("void"); + validTypes.push_back("string"); + validTypes.push_back("int"); + validTypes.push_back("bool"); + validTypes.push_back("char"); + validTypes.push_back("unsigned char"); + validTypes.push_back("size_t"); + validTypes.push_back("double"); + validTypes.push_back("Vector"); + validTypes.push_back("Matrix"); + //Create a list of parsed classes for dependency checking for(const Class& cls: classes) { - validTypes.push_back(cls.qualifiedName("::")); - } - - return validTypes; -} - -/* ************************************************************************* */ -void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes) { - // Generate all collectors + validTypes.push_back(cls.qualifiedName("::")); + } + for(const TypedefPair& p: typedefs) { + validTypes.push_back(p.newType.qualifiedName("::")); + } + + return validTypes; +} + +/* ************************************************************************* */ +void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes) { + // Generate all collectors for(const Class& cls: classes) { - const string matlabUniqueName = cls.qualifiedName(), - cppName = cls.qualifiedName("::"); - wrapperFile.oss << "typedef std::set*> " - << "Collector_" << matlabUniqueName << ";\n"; - wrapperFile.oss << "static Collector_" << matlabUniqueName << - " collector_" << matlabUniqueName << ";\n"; - } - - // generate mexAtExit cleanup function - wrapperFile.oss << - "\nvoid _deleteAllObjects()\n" - "{\n" - " mstream mout;\n" // Send stdout to MATLAB console - " std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n" - " bool anyDeleted = false;\n"; + const string matlabUniqueName = cls.qualifiedName(), + cppName = cls.qualifiedName("::"); + wrapperFile.oss << "typedef std::set*> " + << "Collector_" << matlabUniqueName << ";\n"; + wrapperFile.oss << "static Collector_" << matlabUniqueName << + " collector_" << matlabUniqueName << ";\n"; + } + + // generate mexAtExit cleanup function + wrapperFile.oss << + "\nvoid _deleteAllObjects()\n" + "{\n" + " mstream mout;\n" // Send stdout to MATLAB console + " std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n" + " bool anyDeleted = false;\n"; for(const Class& cls: classes) { - const string matlabUniqueName = cls.qualifiedName(); - const string cppName = cls.qualifiedName("::"); - const string collectorType = "Collector_" + matlabUniqueName; - const string collectorName = "collector_" + matlabUniqueName; - // The extra curly-braces around the for loops work around a limitation in MSVC (existing - // since 2005!) preventing more than 248 blocks. - wrapperFile.oss << - " { for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n" - " iter != " << collectorName << ".end(); ) {\n" - " delete *iter;\n" - " " << collectorName << ".erase(iter++);\n" - " anyDeleted = true;\n" - " } }\n"; - } - wrapperFile.oss << - " if(anyDeleted)\n" - " cout <<\n" - " \"WARNING: Wrap modules with variables in the workspace have been reloaded due to\\n\"\n" - " \"calling destructors, call 'clear all' again if you plan to now recompile a wrap\\n\"\n" - " \"module, so that your recompiled module is used instead of the old one.\" << endl;\n" - " std::cout.rdbuf(outbuf);\n" // Restore cout - "}\n\n"; -} - -/* ************************************************************************* */ -void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes) { - wrapperFile.oss << - "void _" << moduleName << "_RTTIRegister() {\n" - " const mxArray *alreadyCreated = mexGetVariablePtr(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\");\n" - " if(!alreadyCreated) {\n" - " std::map types;\n"; + const string matlabUniqueName = cls.qualifiedName(); + const string cppName = cls.qualifiedName("::"); + const string collectorType = "Collector_" + matlabUniqueName; + const string collectorName = "collector_" + matlabUniqueName; + // The extra curly-braces around the for loops work around a limitation in MSVC (existing + // since 2005!) preventing more than 248 blocks. + wrapperFile.oss << + " { for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n" + " iter != " << collectorName << ".end(); ) {\n" + " delete *iter;\n" + " " << collectorName << ".erase(iter++);\n" + " anyDeleted = true;\n" + " } }\n"; + } + wrapperFile.oss << + " if(anyDeleted)\n" + " cout <<\n" + " \"WARNING: Wrap modules with variables in the workspace have been reloaded due to\\n\"\n" + " \"calling destructors, call 'clear all' again if you plan to now recompile a wrap\\n\"\n" + " \"module, so that your recompiled module is used instead of the old one.\" << endl;\n" + " std::cout.rdbuf(outbuf);\n" // Restore cout + "}\n\n"; +} + +/* ************************************************************************* */ +void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes) { + wrapperFile.oss << + "void _" << moduleName << "_RTTIRegister() {\n" + " const mxArray *alreadyCreated = mexGetVariablePtr(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\");\n" + " if(!alreadyCreated) {\n" + " std::map types;\n"; for(const Class& cls: classes) { - if(cls.isVirtual) - wrapperFile.oss << - " types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName(".") << "\"));\n"; - } - wrapperFile.oss << "\n"; - - wrapperFile.oss << - " mxArray *registry = mexGetVariable(\"global\", \"gtsamwrap_rttiRegistry\");\n" - " if(!registry)\n" - " registry = mxCreateStructMatrix(1, 1, 0, NULL);\n" - " typedef std::pair StringPair;\n" + if(cls.isVirtual) + wrapperFile.oss << + " types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName(".") << "\"));\n"; + } + wrapperFile.oss << "\n"; + + wrapperFile.oss << + " mxArray *registry = mexGetVariable(\"global\", \"gtsamwrap_rttiRegistry\");\n" + " if(!registry)\n" + " registry = mxCreateStructMatrix(1, 1, 0, NULL);\n" + " typedef std::pair StringPair;\n" " for(const StringPair& rtti_matlab: types) {\n" - " int fieldId = mxAddField(registry, rtti_matlab.first.c_str());\n" - " if(fieldId < 0)\n" - " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" - " mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());\n" - " mxSetFieldByNumber(registry, 0, fieldId, matlabName);\n" - " }\n" - " if(mexPutVariable(\"global\", \"gtsamwrap_rttiRegistry\", registry) != 0)\n" - " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" - " mxDestroyArray(registry);\n" - " \n" - " mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);\n" - " if(mexPutVariable(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\", newAlreadyCreated) != 0)\n" - " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" - " mxDestroyArray(newAlreadyCreated);\n" - " }\n" - "}\n" - "\n"; -} - -/* ************************************************************************* */ -void Module::python_wrapper(const string& toolboxPath) const { + " int fieldId = mxAddField(registry, rtti_matlab.first.c_str());\n" + " if(fieldId < 0)\n" + " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" + " mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());\n" + " mxSetFieldByNumber(registry, 0, fieldId, matlabName);\n" + " }\n" + " if(mexPutVariable(\"global\", \"gtsamwrap_rttiRegistry\", registry) != 0)\n" + " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" + " mxDestroyArray(registry);\n" + " \n" + " mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);\n" + " if(mexPutVariable(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\", newAlreadyCreated) != 0)\n" + " mexErrMsgTxt(\"gtsam wrap: Error indexing RTTI types, inheritance will not work correctly\");\n" + " mxDestroyArray(newAlreadyCreated);\n" + " }\n" + "}\n" + "\n"; +} + +/* ************************************************************************* */ +void Module::generate_python_wrapper(const string& toolboxPath) const { fs::create_directories(toolboxPath); @@ -456,8 +625,9 @@ void Module::python_wrapper(const string& toolboxPath) const { wrapperFile.oss << "{\n"; // write out classes - for(const Class& cls: expandedClasses) + for(const Class& cls: expandedClasses) { cls.python_wrapper(wrapperFile); + } // write out global functions for(const GlobalFunctions::value_type& p: global_functions) diff --git a/wrap/Module.h b/wrap/Module.h index e0c1b3f31..732b66507 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -22,6 +22,7 @@ #include "GlobalFunction.h" #include "TemplateInstantiationTypedef.h" #include "ForwardDeclaration.h" +#include "TypedefPair.h" #include #include @@ -38,10 +39,12 @@ struct Module { std::string name; ///< module name bool verbose; ///< verbose flag std::vector classes; ///< list of classes + std::vector uninstantiatedClasses; ///< list of template classes after instantiated std::vector templateInstantiationTypedefs; ///< list of template instantiations std::vector forward_declarations; std::vector includes; ///< Include statements GlobalFunctions global_functions; + std::vector typedefs; // After parsing: std::vector expandedClasses; @@ -60,7 +63,12 @@ struct Module { void parseMarkup(const std::string& data); /// MATLAB code generation: - void matlab_code(const std::string& path) const; + void generate_matlab_wrapper(const std::string& path) const; + + /// Cython code generation: + void generate_cython_wrapper(const std::string& path, const std::string& pxdImports = "") const; + void emit_cython_pxd(FileWriter& file) const; + void emit_cython_pyx(FileWriter& file) const; void generateIncludes(FileWriter& file) const; @@ -68,7 +76,7 @@ struct Module { const std::vector& functionNames) const; /// Python code generation: - void python_wrapper(const std::string& path) const; + void generate_python_wrapper(const std::string& path) const; private: static std::vector ExpandTypedefInstantiations( @@ -76,7 +84,8 @@ private: const std::vector instantiations); static std::vector GenerateValidTypes( const std::vector& classes, - const std::vector forwardDeclarations); + const std::vector& forwardDeclarations, + const std::vector& typedefs); static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes); static void WriteRTTIRegistry(FileWriter& wrapperFile, diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 55e581ad8..6bcb72d94 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -20,36 +20,27 @@ #include "Function.h" #include "Argument.h" - +#include namespace wrap { /** * ArgumentList Overloads */ class ArgumentOverloads { - -protected: - +public: std::vector argLists_; public: + size_t nrOverloads() const { return argLists_.size(); } - size_t nrOverloads() const { - return argLists_.size(); - } + const ArgumentList& argumentList(size_t i) const { return argLists_.at(i); } - const ArgumentList& argumentList(size_t i) const { - return argLists_.at(i); - } - - void push_back(const ArgumentList& args) { - argLists_.push_back(args); - } + void push_back(const ArgumentList& args) { argLists_.push_back(args); } std::vector expandArgumentListsTemplate( const TemplateSubstitution& ts) const { std::vector result; - for(const ArgumentList& argList: argLists_) { + for (const ArgumentList& argList : argLists_) { ArgumentList instArgList = argList.expandTemplate(ts); result.push_back(instArgList); } @@ -62,51 +53,74 @@ public: } void verifyArguments(const std::vector& validArgs, - const std::string s) const { - for(const ArgumentList& argList: argLists_) { - for(Argument arg: argList) { + const std::string s) const { + for (const ArgumentList& argList : argLists_) { + for (Argument arg : argList) { std::string fullType = arg.type.qualifiedName("::"); - if (find(validArgs.begin(), validArgs.end(), fullType) - == validArgs.end()) + if (find(validArgs.begin(), validArgs.end(), fullType) == + validArgs.end()) throw DependencyMissing(fullType, "checking argument of " + s); } } } friend std::ostream& operator<<(std::ostream& os, - const ArgumentOverloads& overloads) { - for(const ArgumentList& argList: overloads.argLists_) + const ArgumentOverloads& overloads) { + for (const ArgumentList& argList : overloads.argLists_) os << argList << std::endl; return os; } + std::string pyx_resolveOverloadParams(const ArgumentList& args, bool isVoid, + size_t indentLevel = 2) const { + std::string indent; + for (size_t i = 0; i < indentLevel; ++i) + indent += " "; + std::string s; + s += indent + "__params = process_args([" + args.pyx_paramsList() + + "], args, kwargs)\n"; + s += args.pyx_castParamsToPythonType(indent); + if (args.size() > 0) { + for (size_t i = 0; i < args.size(); ++i) { + // For python types we can do the assert after the assignment and save list accesses + if (args[i].type.isNonBasicType() || args[i].type.isEigen()) { + std::string param = args[i].name; + s += indent + "assert isinstance(" + param + ", " + + args[i].type.pyxArgumentType() + ")"; + if (args[i].type.isEigen()) { + s += " and " + param + ".ndim == " + + ((args[i].type.pyxClassName() == "Vector") ? "1" : "2"); + } + s += "\n"; + } + } + } + return s; + } }; -class OverloadedFunction: public Function, public ArgumentOverloads { - +class OverloadedFunction : public Function, public ArgumentOverloads { public: - bool addOverload(const std::string& name, const ArgumentList& args, - boost::optional instName = boost::none, bool verbose = - false) { + boost::optional instName = boost::none, + bool verbose = false) { bool first = initializeOrCheck(name, instName, verbose); ArgumentOverloads::push_back(args); return first; } private: - }; // Templated checking functions // TODO: do this via polymorphism, use transform ? -template +template static std::map expandMethodTemplate( const std::map& methods, const TemplateSubstitution& ts) { std::map result; typedef std::pair NamedMethod; - for(NamedMethod namedMethod: methods) { + for (NamedMethod namedMethod : methods) { F instMethod = namedMethod.second; instMethod.expandTemplate(ts); namedMethod.second = instMethod; @@ -115,13 +129,12 @@ static std::map expandMethodTemplate( return result; } -template +template inline void verifyArguments(const std::vector& validArgs, - const std::map& vt) { + const std::map& vt) { typedef typename std::map::value_type NamedMethod; - for(const NamedMethod& namedMethod: vt) + for (const NamedMethod& namedMethod : vt) namedMethod.second.verifyArguments(validArgs); } -} // \namespace wrap - +} // \namespace wrap diff --git a/wrap/Qualified.cpp b/wrap/Qualified.cpp new file mode 100644 index 000000000..947e51d54 --- /dev/null +++ b/wrap/Qualified.cpp @@ -0,0 +1,5 @@ +#include + +namespace wrap { + std::vector Qualified::BasicTypedefs; +} diff --git a/wrap/Qualified.h b/wrap/Qualified.h index bcc4c0829..4dc15dda1 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -21,6 +21,7 @@ #include #include #include +#include namespace wrap { @@ -34,6 +35,7 @@ public: std::vector namespaces_; ///< Stack of namespaces std::string name_; ///< type name + static std::vector BasicTypedefs; friend struct TypeGrammar; friend class TemplateSubstitution; @@ -46,14 +48,23 @@ public: } Category; Category category; + /// Default constructor Qualified() : category(VOID) { } + /// Construct from name and optional category Qualified(const std::string& n, Category c = CLASS) : name_(n), category(c) { } + /// Construct from scoped name and optional category + Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + } + + /// Construct from doubly scoped name and optional category Qualified(const std::string& ns1, const std::string& ns2, const std::string& n, Category c = CLASS) : name_(n), category(c) { @@ -61,15 +72,14 @@ public: namespaces_.push_back(ns2); } - Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) : - name_(n), category(c) { - namespaces_.push_back(ns1); - } - + /// Construct from arbitrarily scoped name Qualified(std::vector ns, const std::string& name) : namespaces_(ns), name_(name), category(CLASS) { } + // Destructor + virtual ~Qualified() {} + std::string name() const { return name_; } @@ -84,6 +94,12 @@ public: return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS); } + bool match(const std::vector& templateArgs) const { + for(const std::string& s: templateArgs) + if (match(s)) return true; + return false; + } + void rename(const Qualified& q) { namespaces_ = q.namespaces_; name_ = q.name_; @@ -109,6 +125,35 @@ public: category = VOID; } + bool isScalar() const { + return (name() == "bool" || name() == "char" + || name() == "unsigned char" || name() == "int" + || name() == "size_t" || name() == "double"); + } + + bool isVoid() const { + return name() == "void"; + } + + bool isString() const { + return name() == "string"; + } + + bool isEigen() const { + return name() == "Vector" || name() == "Matrix"; + } + + bool isBasicTypedef() const { + return std::find(Qualified::BasicTypedefs.begin(), + Qualified::BasicTypedefs.end(), + *this) != Qualified::BasicTypedefs.end(); + } + + bool isNonBasicType() const { + return name() != "This" && !isString() && !isScalar() && !isEigen() && + !isVoid() && !isBasicTypedef(); + } + public: static Qualified MakeClass(std::vector namespaces, @@ -128,10 +173,18 @@ public: return Qualified("void", VOID); } - /// Return a qualified string using given delimiter - std::string qualifiedName(const std::string& delimiter = "") const { + /// Return a qualified namespace using given delimiter + std::string qualifiedNamespaces(const std::string& delimiter = "") const { std::string result; for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += (namespaces_[i] + ((i VectorXd, Matrix --> MatrixXd + std::string pxdClassName() const { + if (isEigen()) + return name_ + "Xd"; + else if (isNonBasicType()) + return "C" + qualifiedName("_", 1); + else return name_; + } + + /// name of Python classes in pyx + /// They have the same name with the corresponding Cython classes in pxd + /// But note that they are different: These are Python classes in the pyx file + /// To refer to a Cython class in pyx, we need to add "pxd.", e.g. pxd.noiseModel_Gaussian + /// see the other function pxd_class_in_pyx for that purpose. + std::string pyxClassName() const { + if (isEigen()) + return name_; + else + return qualifiedName("_", 1); + } + + /// Python type of function arguments in pyx to interface with normal python scripts + /// Eigen types become np.ndarray (There's no Eigen types, e.g. VectorXd, in + /// Python. We have to pass in numpy array in the arguments, which will then be + /// converted to Eigen types in Cython) + std::string pyxArgumentType() const { + if (isEigen()) + return "np.ndarray"; + else + return qualifiedName("_", 1); + } + + /// return the Cython class in pxd corresponding to a Python class in pyx + std::string pxd_class_in_pyx() const { + if (isNonBasicType()) { + return pxdClassName(); + } else if (isEigen()) { + return name_ + "Xd"; + } else // basic types and not Eigen + return name_; + } + + /// the internal Cython shared obj in a Python class wrappper + std::string shared_pxd_obj_in_pyx() const { + return pxdClassName() + "_"; + } + + std::string make_shared_pxd_class_in_pyx() const { + return "make_shared[" + pxd_class_in_pyx() + "]"; + } + + std::string shared_pxd_class_in_pyx() const { + return "shared_ptr[" + pxd_class_in_pyx() + "]"; + } + friend std::ostream& operator<<(std::ostream& os, const Qualified& q) { os << q.qualifiedName("::"); return os; } - }; /* ************************************************************************* */ diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index 1a1f1bc26..506e7d471 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -5,6 +5,7 @@ */ #include "ReturnType.h" +#include "Class.h" #include "utilities.h" #include @@ -18,21 +19,21 @@ string ReturnType::str(bool add_ptr) const { /* ************************************************************************* */ void ReturnType::wrap_result(const string& out, const string& result, - FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const { - + FileWriter& wrapperFile, + const TypeAttributesTable& typeAttributes) const { string cppType = qualifiedName("::"), matlabType = qualifiedName("."); if (category == CLASS) { - // Handle Classes string objCopy, ptrType; const bool isVirtual = typeAttributes.attributes(cppType).isVirtual; if (isPtr) - objCopy = result; // a shared pointer can always be passed as is + objCopy = result; // a shared pointer can always be passed as is else { // but if we want an actual new object, things get more complex if (isVirtual) - // A virtual class needs to be cloned, so the whole hierarchy is returned + // A virtual class needs to be cloned, so the whole hierarchy is + // returned objCopy = result + ".clone()"; else // ...but a non-virtual class can just be copied @@ -40,30 +41,72 @@ void ReturnType::wrap_result(const string& out, const string& result, } // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" - << matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n"; + << matlabType << "\", " << (isVirtual ? "true" : "false") + << ");\n"; } else if (isPtr) { - // Handle shared pointer case for BASIS/EIGEN/VOID - wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" << name() - << "(" << result << ");" << endl; + wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" + << name() << "(" << result << ");" << endl; wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType - << "\");\n }\n"; + << "\");\n }\n"; } else if (matlabType != "void") // Handle normal case case for BASIS/EIGEN wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result - << ");\n"; - + << ");\n"; } /* ************************************************************************* */ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { if (category == CLASS) wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") - << "> Shared" << name() << ";" << endl; + << "> Shared" << name() << ";" << endl; } /* ************************************************************************* */ +void ReturnType::emit_cython_pxd( + FileWriter& file, const std::string& className, + const std::vector& templateArgs) const { + string cythonType; + if (name() == "This") + cythonType = className; + else if (match(templateArgs)) + cythonType = name(); + else + cythonType = pxdClassName(); + if (isPtr) cythonType = "shared_ptr[" + cythonType + "]"; + file.oss << cythonType; +} +/* ************************************************************************* */ +std::string ReturnType::pyx_returnType(bool addShared) const { + string retType = pxd_class_in_pyx(); + if (isPtr || (isNonBasicType() && addShared)) + retType = "shared_ptr[" + retType + "]"; + return retType; +} + +/* ************************************************************************* */ +std::string ReturnType::pyx_casting(const std::string& var, + bool isSharedVar) const { + if (isEigen()) { + string s = "ndarray_copy(" + var + ")"; + if (pyxClassName() == "Vector") + return s + ".squeeze()"; + else return s; + } + else if (isNonBasicType()) { + if (isPtr || isSharedVar) + return pyxClassName() + ".cyCreateFromShared(" + var + ")"; + else { + // construct a shared_ptr if var is not a shared ptr + return pyxClassName() + ".cyCreateFromShared(" + make_shared_pxd_class_in_pyx() + + + "(" + var + "))"; + } + } else + return var; +} + +/* ************************************************************************* */ diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index 1c67a1d9a..de1835f28 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -18,21 +18,17 @@ namespace wrap { /** * Encapsulates return value of a method or function */ -struct ReturnType: public Qualified { - +struct ReturnType : public Qualified { bool isPtr; friend struct ReturnValueGrammar; /// Makes a void type - ReturnType() : - isPtr(false) { - } + ReturnType() : isPtr(false) {} /// Constructor, no namespaces - ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) : - Qualified(name, c), isPtr(ptr) { - } + ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) + : Qualified(name, c), isPtr(ptr) {} virtual void clear() { Qualified::clear(); @@ -40,45 +36,49 @@ struct ReturnType: public Qualified { } /// Check if this type is in a set of valid types - template + template void verify(TYPES validtypes, const std::string& s) const { std::string key = qualifiedName("::"); if (find(validtypes.begin(), validtypes.end(), key) == validtypes.end()) throw DependencyMissing(key, "checking return type of " + s); } -private: + /// @param className the actual class name to use when "This" is specified + void emit_cython_pxd(FileWriter& file, const std::string& className, + const std::vector& templateArgs) const; + std::string pyx_returnType(bool addShared = true) const; + std::string pyx_casting(const std::string& var, + bool isSharedVar = true) const; + +private: friend struct ReturnValue; std::string str(bool add_ptr) const; /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false); void wrap_result(const std::string& out, const std::string& result, - FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; + FileWriter& wrapperFile, + const TypeAttributesTable& typeAttributes) const; /// Creates typedef void wrapTypeUnwrap(FileWriter& wrapperFile) const; - }; //****************************************************************************** // http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html -struct ReturnTypeGrammar: public classic::grammar { - - wrap::ReturnType& result_; ///< successful parse will be placed in here +struct ReturnTypeGrammar : public classic::grammar { + wrap::ReturnType& result_; ///< successful parse will be placed in here TypeGrammar type_g; /// Construct ReturnType grammar and specify where result is placed - ReturnTypeGrammar(wrap::ReturnType& result) : - result_(result), type_g(result_) { - } + ReturnTypeGrammar(wrap::ReturnType& result) + : result_(result), type_g(result_) {} /// Definition of type grammar - template + template struct definition { - classic::rule type_p; definition(ReturnTypeGrammar const& self) { @@ -86,12 +86,9 @@ struct ReturnTypeGrammar: public classic::grammar { type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)]; } - classic::rule const& start() const { - return type_p; - } - + classic::rule const& start() const { return type_p; } }; }; // ReturnTypeGrammar -} // \namespace wrap +} // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 1405e8e2b..3f318eddc 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -17,8 +17,7 @@ using namespace wrap; ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { ReturnValue instRetVal = *this; instRetVal.type1 = ts.tryToSubstitite(type1); - if (isPair) - instRetVal.type2 = ts.tryToSubstitite(type2); + if (isPair) instRetVal.type2 = ts.tryToSubstitite(type2); return instRetVal; } @@ -37,16 +36,17 @@ string ReturnValue::matlab_returnType() const { /* ************************************************************************* */ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, - const TypeAttributesTable& typeAttributes) const { + const TypeAttributesTable& typeAttributes) const { if (isPair) { - // For a pair, store the returned pair so we do not evaluate the function twice + // For a pair, store the returned pair so we do not evaluate the function + // twice wrapperFile.oss << " " << return_type(true) << " pairResult = " << result - << ";\n"; + << ";\n"; type1.wrap_result(" out[0]", "pairResult.first", wrapperFile, - typeAttributes); + typeAttributes); type2.wrap_result(" out[1]", "pairResult.second", wrapperFile, - typeAttributes); - } else { // Not a pair + typeAttributes); + } else { // Not a pair type1.wrap_result(" out[0]", result, wrapperFile, typeAttributes); } } @@ -54,8 +54,7 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, /* ************************************************************************* */ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { type1.wrapTypeUnwrap(wrapperFile); - if (isPair) - type2.wrapTypeUnwrap(wrapperFile); + if (isPair) type2.wrapTypeUnwrap(wrapperFile); } /* ************************************************************************* */ @@ -68,4 +67,41 @@ void ReturnValue::emit_matlab(FileWriter& proxyFile) const { } /* ************************************************************************* */ +void ReturnValue::emit_cython_pxd( + FileWriter& file, const std::string& className, + const std::vector& templateArgs) const { + if (isPair) { + file.oss << "pair["; + type1.emit_cython_pxd(file, className, templateArgs); + file.oss << ","; + type2.emit_cython_pxd(file, className, templateArgs); + file.oss << "] "; + } else { + type1.emit_cython_pxd(file, className, templateArgs); + file.oss << " "; + } +} +/* ************************************************************************* */ +std::string ReturnValue::pyx_returnType() const { + if (isVoid()) return ""; + if (isPair) { + return "pair [" + type1.pyx_returnType(false) + "," + + type2.pyx_returnType(false) + "]"; + } else { + return type1.pyx_returnType(true); + } +} + +/* ************************************************************************* */ +std::string ReturnValue::pyx_casting(const std::string& var) const { + if (isVoid()) return ""; + if (isPair) { + return "(" + type1.pyx_casting(var + ".first", false) + "," + + type2.pyx_casting(var + ".second", false) + ")"; + } else { + return type1.pyx_casting(var); + } +} + +/* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 629684a34..589db5bd6 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -27,27 +27,34 @@ struct ReturnValue { friend struct ReturnValueGrammar; - /// Constructor + /// Default constructor ReturnValue() : isPair(false) { } - /// Constructor + /// Construct from type ReturnValue(const ReturnType& type) : isPair(false), type1(type) { } - /// Constructor + /// Construct from pair type arguments ReturnValue(const ReturnType& t1, const ReturnType& t2) : isPair(true), type1(t1), type2(t2) { } + /// Destructor + virtual ~ReturnValue() {} + virtual void clear() { type1.clear(); type2.clear(); isPair = false; } + bool isVoid() const { + return !isPair && !type1.isPtr && (type1.name() == "void"); + } + bool operator==(const ReturnValue& other) const { return isPair == other.isPair && type1 == other.type1 && type2 == other.type2; @@ -67,6 +74,12 @@ struct ReturnValue { void emit_matlab(FileWriter& proxyFile) const; + /// @param className the actual class name to use when "This" is specified + void emit_cython_pxd(FileWriter& file, const std::string& className, + const std::vector& templateArgs) const; + std::string pyx_returnType() const; + std::string pyx_casting(const std::string& var) const; + friend std::ostream& operator<<(std::ostream& os, const ReturnValue& r) { if (!r.isPair && r.type1.category == ReturnType::VOID) os << "void"; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 23dc93d72..4fe273dee 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -18,6 +18,7 @@ #include "StaticMethod.h" #include "utilities.h" +#include "Class.h" #include #include @@ -56,3 +57,95 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, } /* ************************************************************************* */ +void StaticMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const { + for(size_t i = 0; i < nrOverloads(); ++i) { + file.oss << " @staticmethod\n"; + file.oss << " "; + returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs); + file.oss << name_ + ((i>0)?"_" + to_string(i):"") << " \"" << name_ << "\"" << "("; + argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs); + file.oss << ") except +\n"; + } +} + +/* ************************************************************************* */ +void StaticMethod::emit_cython_wrapper_pxd(FileWriter& file, + const Class& cls) const { + if (nrOverloads() > 1) { + for (size_t i = 0; i < nrOverloads(); ++i) { + string funcName = name_ + "_" + to_string(i); + file.oss << " @staticmethod\n"; + file.oss << " cdef tuple " + funcName + "(tuple args, dict kwargs)\n"; + } + } +} + +/* ************************************************************************* */ +void StaticMethod::emit_cython_pyx_no_overload(FileWriter& file, + const Class& cls) const { + assert(nrOverloads() == 1); + file.oss << " @staticmethod\n"; + file.oss << " def " << name_ << "("; + argumentList(0).emit_cython_pyx(file); + file.oss << "):\n"; + + /// Call cython corresponding function and return + file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder(" "); + string call = pyx_functionCall(cls.pxd_class_in_pyx(), name_, 0); + file.oss << " "; + if (!returnVals_[0].isVoid()) { + file.oss << "return " << returnVals_[0].pyx_casting(call) << "\n"; + } else + file.oss << call << "\n"; + file.oss << "\n"; +} + +/* ************************************************************************* */ +void StaticMethod::emit_cython_pyx(FileWriter& file, const Class& cls) const { + size_t N = nrOverloads(); + if (N == 1) { + emit_cython_pyx_no_overload(file, cls); + return; + } + + // Dealing with overloads.. + file.oss << " @staticmethod # overloaded\n"; + file.oss << " def " << name_ << "(*args, **kwargs):\n"; + for (size_t i = 0; i < N; ++i) { + string funcName = name_ + "_" + to_string(i); + file.oss << " success, results = " << cls.pyxClassName() << "." + << funcName << "(args, kwargs)\n"; + file.oss << " if success:\n return results\n"; + } + file.oss << " raise TypeError('Could not find the correct overload')\n\n"; + + // Create cdef methods for all overloaded methods + for(size_t i = 0; i < N; ++i) { + string funcName = name_ + "_" + to_string(i); + file.oss << " @staticmethod\n"; + file.oss << " cdef tuple " + funcName + "(tuple args, dict kwargs):\n"; + file.oss << " cdef list __params\n"; + if (!returnVals_[i].isVoid()) { + file.oss << " cdef " << returnVals_[i].pyx_returnType() << " return_value\n"; + } + file.oss << " try:\n"; + ArgumentList args = argumentList(i); + file.oss << pyx_resolveOverloadParams(args, false, 3); + + /// Call cython corresponding function and return + file.oss << args.pyx_convertEigenTypeAndStorageOrder(" "); + string pxdFuncName = name_ + ((i>0)?"_" + to_string(i):""); + string call = pyx_functionCall(cls.pxd_class_in_pyx(), pxdFuncName, i); + if (!returnVals_[i].isVoid()) { + file.oss << " return_value = " << call << "\n"; + file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n"; + } else { + file.oss << " " << call << "\n"; + file.oss << " return True, None\n"; + } + file.oss << " except:\n"; + file.oss << " return False, None\n\n"; + } +} + +/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index a01eeff62..cbcfc8d49 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -34,6 +34,11 @@ struct StaticMethod: public MethodBase { return os; } + void emit_cython_pxd(FileWriter& file, const Class& cls) const; + void emit_cython_wrapper_pxd(FileWriter& file, const Class& cls) const; + void emit_cython_pyx(FileWriter& file, const Class& cls) const; + void emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const; + protected: virtual void proxy_header(FileWriter& proxyFile) const; diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index 4c77d4e76..6aa35a5bd 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -60,6 +60,8 @@ Class TemplateInstantiationTypedef::findAndExpand( for (size_t i = 1; i < typeList.size(); ++i) classInst.typedefName += (", " + typeList[i].qualifiedName("::")); classInst.typedefName += ">"; + classInst.templateClass = *matchedClass; + classInst.templateInstTypeList = typeList; return classInst; } diff --git a/wrap/TemplateMethod.cpp b/wrap/TemplateMethod.cpp new file mode 100644 index 000000000..d683fed50 --- /dev/null +++ b/wrap/TemplateMethod.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * 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 TemplateMethod.ccp + * @author Duy-Nguyen Ta + **/ + +#include "TemplateMethod.h" +#include "Class.h" + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void TemplateMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const { + std::vector templateArgs = cls.templateArgs; + templateArgs.push_back(argName); + for(size_t i = 0; i < nrOverloads(); ++i) { + file.oss << " "; + returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), templateArgs); + file.oss << name_ << "[" << argName << "]" << "("; + argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), templateArgs); + file.oss << ") except +\n"; + } +} + +/* ************************************************************************* */ +bool TemplateMethod::addOverload(Str name, const ArgumentList& args, + const ReturnValue& retVal, bool is_const, + std::string _argName, bool verbose) { + argName = _argName; + bool first = MethodBase::addOverload(name, args, retVal, boost::none, verbose); + if (first) + is_const_ = is_const; + else if (is_const && !is_const_) + throw std::runtime_error( + "Method::addOverload now designated as const whereas before it was not"); + else if (!is_const && is_const_) + throw std::runtime_error( + "Method::addOverload now designated as non-const whereas before it was"); + return first; +} + +/* ************************************************************************* */ diff --git a/wrap/TemplateMethod.h b/wrap/TemplateMethod.h new file mode 100644 index 000000000..896074baa --- /dev/null +++ b/wrap/TemplateMethod.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * 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 TemplateMethod.h + * @brief describes and generates code for template methods + * @author Duy-Nguyen Ta + **/ + +#pragma once + +#include "Method.h" + +namespace wrap { + +/// StaticMethod class +struct TemplateMethod: public Method { + std::string argName; // name of template argument + + void emit_cython_pxd(FileWriter& file, const Class& cls) const; + bool addOverload(Str name, const ArgumentList& args, + const ReturnValue& retVal, bool is_const, + std::string argName, bool verbose = false); + + friend std::ostream& operator<<(std::ostream& os, const TemplateMethod& m) { + for (size_t i = 0; i < m.nrOverloads(); i++) + os << "template <" << m.argName << "> " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; + return os; + } + +}; + +} // \namespace wrap + diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp index f98e9b760..ee98480c1 100644 --- a/wrap/TypeAttributesTable.cpp +++ b/wrap/TypeAttributesTable.cpp @@ -18,6 +18,7 @@ #include "TypeAttributesTable.h" #include "Class.h" +#include "ForwardDeclaration.h" #include "utilities.h" #include @@ -42,6 +43,13 @@ const TypeAttributes& TypeAttributesTable::attributes(const string& key) const { } } +/* ************************************************************************* */ +void TypeAttributesTable::addType(const Qualified& cls) { + if (!table_.insert(make_pair(cls.qualifiedName("::"), TypeAttributes(false))) + .second) + throw DuplicateDefinition("types " + cls.qualifiedName("::")); +} + /* ************************************************************************* */ void TypeAttributesTable::addClasses(const vector& classes) { for(const Class& cls: classes) { @@ -55,8 +63,8 @@ void TypeAttributesTable::addClasses(const vector& classes) { void TypeAttributesTable::addForwardDeclarations( const vector& forwardDecls) { for(const ForwardDeclaration& fwDec: forwardDecls) { - if (!table_.insert(make_pair(fwDec.name, TypeAttributes(fwDec.isVirtual))).second) - throw DuplicateDefinition("class " + fwDec.name); + if (!table_.insert(make_pair(fwDec.name(), TypeAttributes(fwDec.isVirtual))).second) + throw DuplicateDefinition("forward defined class " + fwDec.name()); } } diff --git a/wrap/TypeAttributesTable.h b/wrap/TypeAttributesTable.h index 9b1c2acbc..132e2cda1 100644 --- a/wrap/TypeAttributesTable.h +++ b/wrap/TypeAttributesTable.h @@ -21,14 +21,14 @@ #include #include -#include "ForwardDeclaration.h" - #pragma once namespace wrap { // Forward declarations +class Qualified; class Class; +struct ForwardDeclaration; /** Attributes about valid classes, both for classes defined in this module and * also those forward-declared from others. At the moment this only contains @@ -57,6 +57,7 @@ public: } void addClasses(const std::vector& classes); + void addType(const Qualified& types); void addForwardDeclarations( const std::vector& forwardDecls); diff --git a/wrap/TypedefPair.h b/wrap/TypedefPair.h new file mode 100644 index 000000000..c2224eaf2 --- /dev/null +++ b/wrap/TypedefPair.h @@ -0,0 +1,28 @@ +#pragma once + +#include "Qualified.h" + +namespace wrap { +struct TypedefPair { + Qualified oldType, newType; + std::string includeFile; + + TypedefPair() {} + TypedefPair(const Qualified& _oldType, const Qualified& _newType, + const std::string& includeFile) + : oldType(_oldType), newType(_newType), includeFile(includeFile) { + if (!oldType.isNonBasicType() && + std::find(Qualified::BasicTypedefs.begin(), + Qualified::BasicTypedefs.end(), + newType) == Qualified::BasicTypedefs.end()) + Qualified::BasicTypedefs.push_back(newType); + } + + void emit_cython_pxd(FileWriter& file) const { + file.oss << "cdef extern from \"" << includeFile << "\" namespace \"" + << oldType.qualifiedNamespaces("::") << "\":\n"; + file.oss << " ctypedef " << oldType.pxdClassName() << " " + << newType.pxdClassName() << "\n"; + } +}; +} diff --git a/wrap/spirit.h b/wrap/spirit.h index ca081109a..98113a1f4 100644 --- a/wrap/spirit.h +++ b/wrap/spirit.h @@ -15,7 +15,8 @@ #include #include #include - +#include + namespace boost { namespace spirit { diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd new file mode 100644 index 000000000..f2cd513e2 --- /dev/null +++ b/wrap/tests/expected-cython/geometry.pxd @@ -0,0 +1,151 @@ + +from gtsam_eigency.core cimport * +from libcpp.string cimport string +from libcpp.vector cimport vector +from libcpp.pair cimport pair +from libcpp.set cimport set +from libcpp.map cimport map +from libcpp cimport bool + +cdef extern from "boost/shared_ptr.hpp" namespace "boost": + cppclass shared_ptr[T]: + shared_ptr() + shared_ptr(T*) + T* get() + long use_count() const + T& operator*() + + cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r) + cdef shared_ptr[T] make_shared[T](const T& r) + +cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam": + cdef cppclass CPoint2 "gtsam::Point2": + CPoint2() except + + CPoint2(double x, double y) except + + + void argChar(char a) except + + void argUChar(unsigned char a) except + + int dim() except + + void eigenArguments(const VectorXd& v, const MatrixXd& m) except + + char returnChar() except + + CVectorNotEigen vectorConfusion() except + + double x() except + + double y() except + + +cdef class Point2: + cdef shared_ptr[CPoint2] CPoint2_ + @staticmethod + cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other) + + +cdef extern from "gtsam/geometry/Point3.h" namespace "gtsam": + cdef cppclass CPoint3 "gtsam::Point3": + CPoint3(double x, double y, double z) except + + + @staticmethod + CPoint3 StaticFunctionRet "StaticFunctionRet"(double z) except + + @staticmethod + double staticFunction "staticFunction"() except + + + double norm() except + + +cdef class Point3: + cdef shared_ptr[CPoint3] CPoint3_ + @staticmethod + cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other) + + + +cdef extern from "folder/path/to/Test.h": + cdef cppclass CTest "Test": + CTest() except + + CTest(double a, const MatrixXd& b) except + + + void arg_EigenConstRef(const MatrixXd& value) except + + pair[CTest,shared_ptr[CTest]] create_MixedPtrs() except + + pair[shared_ptr[CTest],shared_ptr[CTest]] create_ptrs() except + + void print_ "print"() except + + shared_ptr[CPoint2] return_Point2Ptr(bool value) except + + CTest return_Test(shared_ptr[CTest]& value) except + + shared_ptr[CTest] return_TestPtr(shared_ptr[CTest]& value) except + + bool return_bool(bool value) except + + double return_double(double value) except + + bool return_field(const CTest& t) except + + int return_int(int value) except + + MatrixXd return_matrix1(const MatrixXd& value) except + + MatrixXd return_matrix2(const MatrixXd& value) except + + pair[VectorXd,MatrixXd] return_pair(const VectorXd& v, const MatrixXd& A) except + + pair[shared_ptr[CTest],shared_ptr[CTest]] return_ptrs(shared_ptr[CTest]& p1, shared_ptr[CTest]& p2) except + + size_t return_size_t(size_t value) except + + string return_string(string value) except + + VectorXd return_vector1(const VectorXd& value) except + + VectorXd return_vector2(const VectorXd& value) except + + +cdef class Test: + cdef shared_ptr[CTest] CTest_ + @staticmethod + cdef Test cyCreateFromShared(const shared_ptr[CTest]& other) + + +cdef extern from "folder/path/to/Test.h": + cdef cppclass CMyBase "MyBase": + pass + +cdef class MyBase: + cdef shared_ptr[CMyBase] CMyBase_ + @staticmethod + cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other) + + +cdef extern from "folder/path/to/Test.h": + cdef cppclass CMyTemplate "MyTemplate"[T](CMyBase): + CMyTemplate() except + + + void accept_T(const T& value) except + + void accept_Tptr(shared_ptr[T]& value) except + + pair[T,shared_ptr[T]] create_MixedPtrs() except + + pair[shared_ptr[T],shared_ptr[T]] create_ptrs() except + + T return_T(shared_ptr[T]& value) except + + shared_ptr[T] return_Tptr(shared_ptr[T]& value) except + + pair[shared_ptr[T],shared_ptr[T]] return_ptrs(shared_ptr[T]& p1, shared_ptr[T]& p2) except + + ARG templatedMethod[ARG](const ARG& t) except + + +ctypedef CMyTemplate[CPoint2] CMyTemplatePoint2 + +cdef class MyTemplatePoint2(MyBase): + cdef shared_ptr[CMyTemplatePoint2] CMyTemplatePoint2_ + @staticmethod + cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other) + +ctypedef CMyTemplate[MatrixXd] CMyTemplateMatrix + +cdef class MyTemplateMatrix(MyBase): + cdef shared_ptr[CMyTemplateMatrix] CMyTemplateMatrix_ + @staticmethod + cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other) + + +cdef extern from "folder/path/to/Test.h": + cdef cppclass CMyFactor "MyFactor"[POSE,POINT]: + CMyFactor(size_t key1, size_t key2, double measured, const shared_ptr[CnoiseModel_Base]& noiseModel) except + + + +ctypedef CMyFactor[CPose2, MatrixXd] CMyFactorPosePoint2 + +cdef class MyFactorPosePoint2: + cdef shared_ptr[CMyFactorPosePoint2] CMyFactorPosePoint2_ + @staticmethod + cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other) + + +cdef extern from "folder/path/to/Test.h": + cdef cppclass CMyVector "MyVector"[N]: + CMyVector() except + + + + +cdef extern from "folder/path/to/Test.h" namespace "": + VectorXd pxd_aGlobalFunction "aGlobalFunction"() +cdef extern from "folder/path/to/Test.h" namespace "": + VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a) + VectorXd pxd_overloadedGlobalFunction "overloadedGlobalFunction"(int a, double b) diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx new file mode 100644 index 000000000..4bd14b130 --- /dev/null +++ b/wrap/tests/expected-cython/geometry.pyx @@ -0,0 +1,481 @@ +cimport numpy as np +import numpy as npp +cimport geometry +from geometry cimport shared_ptr +from geometry cimport dynamic_pointer_cast +from geometry cimport make_shared +# C helper function that copies all arguments into a positional list. +cdef list process_args(list keywords, tuple args, dict kwargs): + cdef str keyword + cdef int n = len(args), m = len(keywords) + cdef list params = list(args) + assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m) + try: + return params + [kwargs[keyword] for keyword in keywords[n:]] + except: + raise ValueError('Epected arguments ' + str(keywords)) +from gtsam_eigency.core cimport * +from libcpp cimport bool + +from libcpp.pair cimport pair +from libcpp.string cimport string +from cython.operator cimport dereference as deref + + +cdef class Point2: + def __init__(self, *args, **kwargs): + cdef list __params + self.CPoint2_ = shared_ptr[CPoint2]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args([], args, kwargs) + self.CPoint2_ = shared_ptr[CPoint2](new CPoint2()) + except (AssertionError, ValueError): + pass + try: + __params = process_args(['x', 'y'], args, kwargs) + x = (__params[0]) + y = (__params[1]) + self.CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y)) + except (AssertionError, ValueError): + pass + if (self.CPoint2_.use_count()==0): + raise TypeError('Point2 construction failed!') + + @staticmethod + cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef Point2 return_value = Point2(cyCreateFromShared=True) + return_value.CPoint2_ = other + return return_value + + def argChar(self, char a): + self.CPoint2_.get().argChar(a) + def argUChar(self, unsigned char a): + self.CPoint2_.get().argUChar(a) + def dim(self): + cdef int ret = self.CPoint2_.get().dim() + return ret + def eigenArguments(self, np.ndarray v, np.ndarray m): + v = v.astype(float, order='F', copy=False) + m = m.astype(float, order='F', copy=False) + self.CPoint2_.get().eigenArguments((Map[VectorXd](v)), (Map[MatrixXd](m))) + def returnChar(self): + cdef char ret = self.CPoint2_.get().returnChar() + return ret + def vectorConfusion(self): + cdef shared_ptr[CVectorNotEigen] ret = make_shared[CVectorNotEigen](self.CPoint2_.get().vectorConfusion()) + return VectorNotEigen.cyCreateFromShared(ret) + def x(self): + cdef double ret = self.CPoint2_.get().x() + return ret + def y(self): + cdef double ret = self.CPoint2_.get().y() + return ret + + +cdef class Point3: + def __init__(self, *args, **kwargs): + cdef list __params + self.CPoint3_ = shared_ptr[CPoint3]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args(['x', 'y', 'z'], args, kwargs) + x = (__params[0]) + y = (__params[1]) + z = (__params[2]) + self.CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z)) + except (AssertionError, ValueError): + pass + if (self.CPoint3_.use_count()==0): + raise TypeError('Point3 construction failed!') + + @staticmethod + cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef Point3 return_value = Point3(cyCreateFromShared=True) + return_value.CPoint3_ = other + return return_value + + @staticmethod + def StaticFunctionRet(double z): + return Point3.cyCreateFromShared(make_shared[CPoint3](CPoint3.StaticFunctionRet(z))) + + @staticmethod + def staticFunction(): + return CPoint3.staticFunction() + + + def norm(self): + cdef double ret = self.CPoint3_.get().norm() + return ret + + +cdef class Test: + def __init__(self, *args, **kwargs): + cdef list __params + self.CTest_ = shared_ptr[CTest]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args([], args, kwargs) + self.CTest_ = shared_ptr[CTest](new CTest()) + except (AssertionError, ValueError): + pass + try: + __params = process_args(['a', 'b'], args, kwargs) + a = (__params[0]) + b = (__params[1]) + assert isinstance(b, np.ndarray) and b.ndim == 2 + b = b.astype(float, order='F', copy=False) + self.CTest_ = shared_ptr[CTest](new CTest(a, (Map[MatrixXd](b)))) + except (AssertionError, ValueError): + pass + if (self.CTest_.use_count()==0): + raise TypeError('Test construction failed!') + + @staticmethod + cdef Test cyCreateFromShared(const shared_ptr[CTest]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef Test return_value = Test(cyCreateFromShared=True) + return_value.CTest_ = other + return return_value + + def arg_EigenConstRef(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + self.CTest_.get().arg_EigenConstRef((Map[MatrixXd](value))) + def create_MixedPtrs(self): + cdef pair [CTest,shared_ptr[CTest]] ret = self.CTest_.get().create_MixedPtrs() + return (Test.cyCreateFromShared(make_shared[CTest](ret.first)),Test.cyCreateFromShared(ret.second)) + def create_ptrs(self): + cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().create_ptrs() + return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second)) + def __repr__(self): + strBuf = RedirectCout() + self.print_('') + return strBuf.str() + def print_(self): + self.CTest_.get().print_() + def return_Point2Ptr(self, bool value): + cdef shared_ptr[CPoint2] ret = self.CTest_.get().return_Point2Ptr(value) + return Point2.cyCreateFromShared(ret) + def return_Test(self, Test value): + cdef shared_ptr[CTest] ret = make_shared[CTest](self.CTest_.get().return_Test(value.CTest_)) + return Test.cyCreateFromShared(ret) + def return_TestPtr(self, Test value): + cdef shared_ptr[CTest] ret = self.CTest_.get().return_TestPtr(value.CTest_) + return Test.cyCreateFromShared(ret) + def return_bool(self, bool value): + cdef bool ret = self.CTest_.get().return_bool(value) + return ret + def return_double(self, double value): + cdef double ret = self.CTest_.get().return_double(value) + return ret + def return_field(self, Test t): + cdef bool ret = self.CTest_.get().return_field(deref(t.CTest_)) + return ret + def return_int(self, int value): + cdef int ret = self.CTest_.get().return_int(value) + return ret + def return_matrix1(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + cdef MatrixXd ret = self.CTest_.get().return_matrix1((Map[MatrixXd](value))) + return ndarray_copy(ret) + def return_matrix2(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + cdef MatrixXd ret = self.CTest_.get().return_matrix2((Map[MatrixXd](value))) + return ndarray_copy(ret) + def return_pair(self, np.ndarray v, np.ndarray A): + v = v.astype(float, order='F', copy=False) + A = A.astype(float, order='F', copy=False) + cdef pair [VectorXd,MatrixXd] ret = self.CTest_.get().return_pair((Map[VectorXd](v)), (Map[MatrixXd](A))) + return (ndarray_copy(ret.first).squeeze(),ndarray_copy(ret.second)) + def return_ptrs(self, Test p1, Test p2): + cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().return_ptrs(p1.CTest_, p2.CTest_) + return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second)) + def return_size_t(self, size_t value): + cdef size_t ret = self.CTest_.get().return_size_t(value) + return ret + def return_string(self, string value): + cdef string ret = self.CTest_.get().return_string(value) + return ret + def return_vector1(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + cdef VectorXd ret = self.CTest_.get().return_vector1((Map[VectorXd](value))) + return ndarray_copy(ret).squeeze() + def return_vector2(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + cdef VectorXd ret = self.CTest_.get().return_vector2((Map[VectorXd](value))) + return ndarray_copy(ret).squeeze() + + +cdef class MyBase: + def __init__(self, *args, **kwargs): + cdef list __params + self.CMyBase_ = shared_ptr[CMyBase]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + if (self.CMyBase_.use_count()==0): + raise TypeError('MyBase construction failed!') + + @staticmethod + cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef MyBase return_value = MyBase(cyCreateFromShared=True) + return_value.CMyBase_ = other + return return_value + + + +cdef class MyTemplatePoint2(MyBase): + def __init__(self, *args, **kwargs): + cdef list __params + self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args([], args, kwargs) + self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2()) + except (AssertionError, ValueError): + pass + if (self.CMyTemplatePoint2_.use_count()==0): + raise TypeError('MyTemplatePoint2 construction failed!') + self.CMyBase_ = (self.CMyTemplatePoint2_) + + @staticmethod + cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef MyTemplatePoint2 return_value = MyTemplatePoint2(cyCreateFromShared=True) + return_value.CMyTemplatePoint2_ = other + return_value.CMyBase_ = (other) + return return_value + + def accept_T(self, Point2 value): + self.CMyTemplatePoint2_.get().accept_T(deref(value.CPoint2_)) + def accept_Tptr(self, Point2 value): + self.CMyTemplatePoint2_.get().accept_Tptr(value.CPoint2_) + def create_MixedPtrs(self): + cdef pair [CPoint2,shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_MixedPtrs() + return (Point2.cyCreateFromShared(make_shared[CPoint2](ret.first)),Point2.cyCreateFromShared(ret.second)) + def create_ptrs(self): + cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_ptrs() + return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second)) + def return_T(self, Point2 value): + cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().return_T(value.CPoint2_)) + return Point2.cyCreateFromShared(ret) + def return_Tptr(self, Point2 value): + cdef shared_ptr[CPoint2] ret = self.CMyTemplatePoint2_.get().return_Tptr(value.CPoint2_) + return Point2.cyCreateFromShared(ret) + def return_ptrs(self, Point2 p1, Point2 p2): + cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().return_ptrs(p1.CPoint2_, p2.CPoint2_) + return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second)) + def templatedMethodMatrix(self, np.ndarray t): + t = t.astype(float, order='F', copy=False) + cdef MatrixXd ret = self.CMyTemplatePoint2_.get().templatedMethod[MatrixXd]((Map[MatrixXd](t))) + return ndarray_copy(ret) + def templatedMethodPoint2(self, Point2 t): + cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().templatedMethod[CPoint2](deref(t.CPoint2_))) + return Point2.cyCreateFromShared(ret) + def templatedMethodPoint3(self, Point3 t): + cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplatePoint2_.get().templatedMethod[CPoint3](deref(t.CPoint3_))) + return Point3.cyCreateFromShared(ret) + def templatedMethodVector(self, np.ndarray t): + t = t.astype(float, order='F', copy=False) + cdef VectorXd ret = self.CMyTemplatePoint2_.get().templatedMethod[VectorXd]((Map[VectorXd](t))) + return ndarray_copy(ret).squeeze() +def dynamic_cast_MyTemplatePoint2_MyBase(MyBase parent): + try: + return MyTemplatePoint2.cyCreateFromShared(dynamic_pointer_cast[CMyTemplatePoint2,CMyBase](parent.CMyBase_)) + except: + raise TypeError('dynamic cast failed!') + + +cdef class MyTemplateMatrix(MyBase): + def __init__(self, *args, **kwargs): + cdef list __params + self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args([], args, kwargs) + self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix()) + except (AssertionError, ValueError): + pass + if (self.CMyTemplateMatrix_.use_count()==0): + raise TypeError('MyTemplateMatrix construction failed!') + self.CMyBase_ = (self.CMyTemplateMatrix_) + + @staticmethod + cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef MyTemplateMatrix return_value = MyTemplateMatrix(cyCreateFromShared=True) + return_value.CMyTemplateMatrix_ = other + return_value.CMyBase_ = (other) + return return_value + + def accept_T(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + self.CMyTemplateMatrix_.get().accept_T((Map[MatrixXd](value))) + def accept_Tptr(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + self.CMyTemplateMatrix_.get().accept_Tptr((Map[MatrixXd](value))) + def create_MixedPtrs(self): + cdef pair [MatrixXd,shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_MixedPtrs() + return (ndarray_copy(ret.first),ndarray_copy(ret.second)) + def create_ptrs(self): + cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_ptrs() + return (ndarray_copy(ret.first),ndarray_copy(ret.second)) + def return_T(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + cdef MatrixXd ret = self.CMyTemplateMatrix_.get().return_T((Map[MatrixXd](value))) + return ndarray_copy(ret) + def return_Tptr(self, np.ndarray value): + value = value.astype(float, order='F', copy=False) + cdef shared_ptr[MatrixXd] ret = self.CMyTemplateMatrix_.get().return_Tptr((Map[MatrixXd](value))) + return ndarray_copy(ret) + def return_ptrs(self, np.ndarray p1, np.ndarray p2): + p1 = p1.astype(float, order='F', copy=False) + p2 = p2.astype(float, order='F', copy=False) + cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().return_ptrs((Map[MatrixXd](p1)), (Map[MatrixXd](p2))) + return (ndarray_copy(ret.first),ndarray_copy(ret.second)) + def templatedMethodMatrix(self, np.ndarray t): + t = t.astype(float, order='F', copy=False) + cdef MatrixXd ret = self.CMyTemplateMatrix_.get().templatedMethod[MatrixXd]((Map[MatrixXd](t))) + return ndarray_copy(ret) + def templatedMethodPoint2(self, Point2 t): + cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplateMatrix_.get().templatedMethod[CPoint2](deref(t.CPoint2_))) + return Point2.cyCreateFromShared(ret) + def templatedMethodPoint3(self, Point3 t): + cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplateMatrix_.get().templatedMethod[CPoint3](deref(t.CPoint3_))) + return Point3.cyCreateFromShared(ret) + def templatedMethodVector(self, np.ndarray t): + t = t.astype(float, order='F', copy=False) + cdef VectorXd ret = self.CMyTemplateMatrix_.get().templatedMethod[VectorXd]((Map[VectorXd](t))) + return ndarray_copy(ret).squeeze() +def dynamic_cast_MyTemplateMatrix_MyBase(MyBase parent): + try: + return MyTemplateMatrix.cyCreateFromShared(dynamic_pointer_cast[CMyTemplateMatrix,CMyBase](parent.CMyBase_)) + except: + raise TypeError('dynamic cast failed!') + + +cdef class MyVector3: + def __init__(self, *args, **kwargs): + cdef list __params + self.CMyVector3_ = shared_ptr[CMyVector3]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args([], args, kwargs) + self.CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3()) + except (AssertionError, ValueError): + pass + if (self.CMyVector3_.use_count()==0): + raise TypeError('MyVector3 construction failed!') + + @staticmethod + cdef MyVector3 cyCreateFromShared(const shared_ptr[CMyVector3]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef MyVector3 return_value = MyVector3(cyCreateFromShared=True) + return_value.CMyVector3_ = other + return return_value + + + +cdef class MyVector12: + def __init__(self, *args, **kwargs): + cdef list __params + self.CMyVector12_ = shared_ptr[CMyVector12]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args([], args, kwargs) + self.CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12()) + except (AssertionError, ValueError): + pass + if (self.CMyVector12_.use_count()==0): + raise TypeError('MyVector12 construction failed!') + + @staticmethod + cdef MyVector12 cyCreateFromShared(const shared_ptr[CMyVector12]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef MyVector12 return_value = MyVector12(cyCreateFromShared=True) + return_value.CMyVector12_ = other + return return_value + + + +cdef class MyFactorPosePoint2: + def __init__(self, *args, **kwargs): + cdef list __params + self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2]() + if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): + return + try: + __params = process_args(['key1', 'key2', 'measured', 'noiseModel'], args, kwargs) + key1 = (__params[0]) + key2 = (__params[1]) + measured = (__params[2]) + noiseModel = (__params[3]) + assert isinstance(noiseModel, noiseModel_Base) + self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.CnoiseModel_Base_)) + except (AssertionError, ValueError): + pass + if (self.CMyFactorPosePoint2_.use_count()==0): + raise TypeError('MyFactorPosePoint2 construction failed!') + + @staticmethod + cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other): + if other.get() == NULL: + raise RuntimeError('Cannot create object from a nullptr!') + cdef MyFactorPosePoint2 return_value = MyFactorPosePoint2(cyCreateFromShared=True) + return_value.CMyFactorPosePoint2_ = other + return return_value + + + + +def aGlobalFunction(): + cdef VectorXd ret = pxd_aGlobalFunction() + return ndarray_copy(ret).squeeze() +def overloadedGlobalFunction(*args, **kwargs): + success, results = overloadedGlobalFunction_0(args, kwargs) + if success: + return results + success, results = overloadedGlobalFunction_1(args, kwargs) + if success: + return results + raise TypeError('Could not find the correct overload') +def overloadedGlobalFunction_0(args, kwargs): + cdef list __params + cdef VectorXd return_value + try: + __params = process_args(['a'], args, kwargs) + a = (__params[0]) + return_value = pxd_overloadedGlobalFunction(a) + return True, ndarray_copy(return_value).squeeze() + except: + return False, None + +def overloadedGlobalFunction_1(args, kwargs): + cdef list __params + cdef VectorXd return_value + try: + __params = process_args(['a', 'b'], args, kwargs) + a = (__params[0]) + b = (__params[1]) + return_value = pxd_overloadedGlobalFunction(a, b) + return True, ndarray_copy(return_value).squeeze() + except: + return False, None + diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index ef9051d14..7e0cb0e47 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; diff --git a/wrap/tests/expected2/MyFactorPosePoint2.m b/wrap/tests/expected2/MyFactorPosePoint2.m index 430206232..a61e54eb2 100644 --- a/wrap/tests/expected2/MyFactorPosePoint2.m +++ b/wrap/tests/expected2/MyFactorPosePoint2.m @@ -12,9 +12,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - geometry_wrapper(74, my_ptr); + geometry_wrapper(80, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = geometry_wrapper(75, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = geometry_wrapper(81, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -22,7 +22,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - geometry_wrapper(76, obj.ptr_MyFactorPosePoint2); + geometry_wrapper(82, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected2/MyVector12.m b/wrap/tests/expected2/MyVector12.m new file mode 100644 index 000000000..4bd86e8a7 --- /dev/null +++ b/wrap/tests/expected2/MyVector12.m @@ -0,0 +1,36 @@ +%class MyVector12, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%MyVector12() +% +classdef MyVector12 < handle + properties + ptr_MyVector12 = 0 + end + methods + function obj = MyVector12(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(77, my_ptr); + elseif nargin == 0 + my_ptr = geometry_wrapper(78); + else + error('Arguments do not match any overload of MyVector12 constructor'); + end + obj.ptr_MyVector12 = my_ptr; + end + + function delete(obj) + geometry_wrapper(79, obj.ptr_MyVector12); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/MyVector3.m b/wrap/tests/expected2/MyVector3.m new file mode 100644 index 000000000..82f3ed4bd --- /dev/null +++ b/wrap/tests/expected2/MyVector3.m @@ -0,0 +1,36 @@ +%class MyVector3, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%MyVector3() +% +classdef MyVector3 < handle + properties + ptr_MyVector3 = 0 + end + methods + function obj = MyVector3(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(74, my_ptr); + elseif nargin == 0 + my_ptr = geometry_wrapper(75); + else + error('Arguments do not match any overload of MyVector3 constructor'); + end + obj.ptr_MyVector3 = my_ptr; + end + + function delete(obj) + geometry_wrapper(76, obj.ptr_MyVector3); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/aGlobalFunction.m b/wrap/tests/expected2/aGlobalFunction.m index d96662dc1..7246b478b 100644 --- a/wrap/tests/expected2/aGlobalFunction.m +++ b/wrap/tests/expected2/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) if length(varargin) == 0 - varargout{1} = geometry_wrapper(77, varargin{:}); + varargout{1} = geometry_wrapper(83, varargin{:}); else error('Arguments do not match any overload of function aGlobalFunction'); end diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index 8526900a7..ba06d1309 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -2,9 +2,13 @@ #include #include +#include +#include typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; +typedef MyVector<3> MyVector3; +typedef MyVector<12> MyVector12; typedef MyFactor MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -19,6 +23,10 @@ typedef std::set*> Collector_MyTemplatePoint static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; typedef std::set*> Collector_MyTemplateMatrix; static Collector_MyTemplateMatrix collector_MyTemplateMatrix; +typedef std::set*> Collector_MyVector3; +static Collector_MyVector3 collector_MyVector3; +typedef std::set*> Collector_MyVector12; +static Collector_MyVector12 collector_MyVector12; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -64,6 +72,18 @@ void _deleteAllObjects() collector_MyTemplateMatrix.erase(iter++); anyDeleted = true; } } + { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); + iter != collector_MyVector3.end(); ) { + delete *iter; + collector_MyVector3.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); + iter != collector_MyVector12.end(); ) { + delete *iter; + collector_MyVector12.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -885,7 +905,73 @@ void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin out[0] = wrap< Vector >(obj->templatedMethod(t)); } -void MyFactorPosePoint2_collectorInsertAndMakeBase_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_collectorInsertAndMakeBase_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyVector3.insert(self); +} + +void MyVector3_constructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new MyVector3()); + collector_MyVector3.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void MyVector3_deconstructor_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyVector3",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyVector3::iterator item; + item = collector_MyVector3.find(self); + if(item != collector_MyVector3.end()) { + delete self; + collector_MyVector3.erase(item); + } +} + +void MyVector12_collectorInsertAndMakeBase_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyVector12.insert(self); +} + +void MyVector12_constructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new MyVector12()); + collector_MyVector12.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void MyVector12_deconstructor_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyVector12",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyVector12::iterator item; + item = collector_MyVector12.find(self); + if(item != collector_MyVector12.end()) { + delete self; + collector_MyVector12.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_80(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -894,7 +980,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_74(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr Shared; @@ -909,7 +995,7 @@ void MyFactorPosePoint2_constructor_75(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_82(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -922,18 +1008,18 @@ void MyFactorPosePoint2_deconstructor_76(int nargout, mxArray *out[], int nargin } } -void aGlobalFunction_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void aGlobalFunction_83(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } -void overloadedGlobalFunction_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void overloadedGlobalFunction_84(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("overloadedGlobalFunction",nargout,nargin,1); int a = unwrap< int >(in[0]); out[0] = wrap< Vector >(overloadedGlobalFunction(a)); } -void overloadedGlobalFunction_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void overloadedGlobalFunction_85(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("overloadedGlobalFunction",nargout,nargin,2); int a = unwrap< int >(in[0]); @@ -1175,22 +1261,40 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_templatedMethod_73(nargout, out, nargin-1, in+1); break; case 74: - MyFactorPosePoint2_collectorInsertAndMakeBase_74(nargout, out, nargin-1, in+1); + MyVector3_collectorInsertAndMakeBase_74(nargout, out, nargin-1, in+1); break; case 75: - MyFactorPosePoint2_constructor_75(nargout, out, nargin-1, in+1); + MyVector3_constructor_75(nargout, out, nargin-1, in+1); break; case 76: - MyFactorPosePoint2_deconstructor_76(nargout, out, nargin-1, in+1); + MyVector3_deconstructor_76(nargout, out, nargin-1, in+1); break; case 77: - aGlobalFunction_77(nargout, out, nargin-1, in+1); + MyVector12_collectorInsertAndMakeBase_77(nargout, out, nargin-1, in+1); break; case 78: - overloadedGlobalFunction_78(nargout, out, nargin-1, in+1); + MyVector12_constructor_78(nargout, out, nargin-1, in+1); break; case 79: - overloadedGlobalFunction_79(nargout, out, nargin-1, in+1); + MyVector12_deconstructor_79(nargout, out, nargin-1, in+1); + break; + case 80: + MyFactorPosePoint2_collectorInsertAndMakeBase_80(nargout, out, nargin-1, in+1); + break; + case 81: + MyFactorPosePoint2_constructor_81(nargout, out, nargin-1, in+1); + break; + case 82: + MyFactorPosePoint2_deconstructor_82(nargout, out, nargin-1, in+1); + break; + case 83: + aGlobalFunction_83(nargout, out, nargin-1, in+1); + break; + case 84: + overloadedGlobalFunction_84(nargout, out, nargin-1, in+1); + break; + case 85: + overloadedGlobalFunction_85(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/wrap/tests/expected2/overloadedGlobalFunction.m b/wrap/tests/expected2/overloadedGlobalFunction.m index 7dd7317ab..e5b4b21a9 100644 --- a/wrap/tests/expected2/overloadedGlobalFunction.m +++ b/wrap/tests/expected2/overloadedGlobalFunction.m @@ -1,8 +1,8 @@ function varargout = overloadedGlobalFunction(varargin) if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(78, varargin{:}); + varargout{1} = geometry_wrapper(84, varargin{:}); elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') - varargout{1} = geometry_wrapper(79, varargin{:}); + varargout{1} = geometry_wrapper(85, varargin{:}); else error('Arguments do not match any overload of function overloadedGlobalFunction'); end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 376e39b62..74db80c81 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -5,6 +5,7 @@ virtual class ns::OtherClass; namespace gtsam { +#include class Point2 { Point2(); Point2(double x, double y); @@ -20,6 +21,7 @@ class Point2 { void serializable() const; // Sets flag and creates export, but does not make serialization functions }; +#include class Point3 { Point3(double x, double y, double z); double norm() const; diff --git a/wrap/tests/testGlobalFunction.cpp b/wrap/tests/testGlobalFunction.cpp index 32ab5dafb..e707b6ee7 100644 --- a/wrap/tests/testGlobalFunction.cpp +++ b/wrap/tests/testGlobalFunction.cpp @@ -37,7 +37,8 @@ TEST( GlobalFunction, Grammar ) { // Create type grammar that will place result in actual GlobalFunctions actual; vector namespaces; - GlobalFunctionGrammar g(actual,namespaces); + std::string includeFile; + GlobalFunctionGrammar g(actual,namespaces,includeFile); // a class type with namespaces EXPECT(parse("Vector aGlobalFunction();", g, space_p).full); diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 3f6ccb9f1..8668ec88f 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -60,11 +60,11 @@ TEST( wrap, check_exception ) { THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); -// // TODO: matlab_code does not throw this anymore, so check constructor +// // TODO: generate_matlab_wrapper does not throw this anymore, so check constructor // fs::remove_all("actual_deps"); // clean out previous generated code // string path = topdir + "/wrap/tests"; // Module module(path.c_str(), "testDependencies",enable_verbose); -// CHECK_EXCEPTION(module.matlab_code("actual_deps"), DependencyMissing); +// CHECK_EXCEPTION(module.generate_matlab_wrapper("actual_deps"), DependencyMissing); } /* ************************************************************************* */ @@ -148,11 +148,14 @@ TEST( wrap, Geometry ) { // forward declarations LONGS_EQUAL(2, module.forward_declarations.size()); - EXPECT(assert_equal("VectorNotEigen", module.forward_declarations[0].name)); - EXPECT(assert_equal("ns::OtherClass", module.forward_declarations[1].name)); + EXPECT(assert_equal("VectorNotEigen", module.forward_declarations[0].name())); + EXPECT(assert_equal("ns::OtherClass", module.forward_declarations[1].name())); // includes - strvec exp_includes; exp_includes += "folder/path/to/Test.h"; + strvec exp_includes; + exp_includes += "gtsam/geometry/Point2.h"; + exp_includes += "gtsam/geometry/Point3.h"; + exp_includes += "folder/path/to/Test.h"; EXPECT(assert_equal(exp_includes, module.includes)); LONGS_EQUAL(9, module.classes.size()); @@ -408,7 +411,7 @@ TEST( wrap, matlab_code_namespaces ) { // emit MATLAB code string exp_path = path + "/tests/expected_namespaces/"; string act_path = "actual_namespaces/"; - module.matlab_code("actual_namespaces"); + module.generate_matlab_wrapper("actual_namespaces"); EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); @@ -436,7 +439,7 @@ TEST( wrap, matlab_code_geometry ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual"); + module.generate_matlab_wrapper("actual"); #ifndef WRAP_DISABLE_SERIALIZE string epath = path + "/tests/expected/"; #else @@ -470,13 +473,33 @@ TEST( wrap, python_code_geometry ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.python_wrapper("actual-python"); + module.generate_python_wrapper("actual-python"); string epath = path + "/tests/expected-python/"; string apath = "actual-python/"; EXPECT(files_equal(epath + "geometry_python.cpp", apath + "geometry_python.cpp" )); } +/* ************************************************************************* */ +TEST( wrap, cython_code_geometry ) { + // Parse into class object + string header_path = topdir + "/wrap/tests"; + Module module(header_path,"geometry",enable_verbose); + string path = topdir + "/wrap"; + + // clean out previous generated code + fs::remove_all("actual-python"); + + // emit MATLAB code + // make_geometry will not compile, use make testwrap to generate real make + module.generate_cython_wrapper("actual-cython"); + string epath = path + "/tests/expected-cython/"; + string apath = "actual-cython/"; + + EXPECT(files_equal(epath + "geometry.pxd", apath + "geometry.pxd" )); + EXPECT(files_equal(epath + "geometry.pyx", apath + "geometry.pyx" )); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 2e5ac1612..fe17e1c66 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -22,35 +22,47 @@ using namespace std; +/** Displays usage information */ +void usage() { + cerr << "wrap parses an interface file and produces a MATLAB or Cython toolbox" << endl; + cerr << "usage: wrap [--matlab|--cython] interfacePath moduleName toolboxPath cythonImports" << endl; + cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; + cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; + cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; + cerr << " cythonImports : extra imports for Cython pxd header file" << endl; +} + /** * Top-level function to wrap a module + * @param language can be "--matlab" or "--cython" * @param interfacePath path to where interface file lives, e.g., borg/gtsam * @param moduleName name of the module to be generated e.g. gtsam * @param toolboxPath path where the toolbox should be generated, e.g. borg/gtsam/build * @param headerPath is the path to matlab.h + * @param cythonImports additional imports to include in the generated Cython pxd header file */ -void generate_matlab_toolbox( +void generate_toolbox( + const string& language, const string& interfacePath, const string& moduleName, const string& toolboxPath, - const string& headerPath) + const string& cythonImports) { // Parse interface file into class object // This recursively creates Class objects, Method objects, etc... wrap::Module module(interfacePath, moduleName, false); + if (language == "--matlab") // Then emit MATLAB code - module.matlab_code(toolboxPath); -} - -/** Displays usage information */ -void usage() { - cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; - cerr << "usage: wrap interfacePath moduleName toolboxPath headerPath" << endl; - cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; - cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; - cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; - cerr << " headerPath : path to matlab.h" << endl; + module.generate_matlab_wrapper(toolboxPath); + else if (language == "--cython") { + module.generate_cython_wrapper(toolboxPath, cythonImports); + } + else { + cerr << "First argument invalid" << endl; + cerr << endl; + usage(); + } } /** @@ -58,7 +70,7 @@ void usage() { * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { - if (argc != 5) { + if (argc != 6) { cerr << "Invalid arguments:\n"; for (int i=0; i