diff --git a/.gitignore b/.gitignore index b3ea282dd..32f124c43 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,9 @@ *.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 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..2632fbe2e 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,9 @@ In GTSAM 4 a new and more efficient implementation, based on integrating on the Additional Information ---------------------- -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. +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/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..6f378f77d --- /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 -a -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} 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/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_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/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..45ac6caaa --- /dev/null +++ b/cython/gtsam_eigency/eigency_cpp.h @@ -0,0 +1,452 @@ +#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 + ) {} +}; + + +template class DenseBase, + 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) {} + + 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)) { + } + + 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])) { + + 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."); + } + FlattenedMap &operator=(const FlattenedMap &other) { + // 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 DenseBase() const { + return DenseBase(static_cast(*this)); + } +}; + + +template +class Map: public MapBase { +public: + typedef MapBase Base; + typedef typename MatrixType::Scalar Scalar; + + Map() + : Base(NULL, 0, 0) { + } + + Map(Scalar *data, long rows, long cols) + : Base(data, rows, cols) {} + + 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_C_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_C_CONTIGUOUS(object) + ? object->dimensions[0] + : ((object->nd == 1) + ? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector) + : object->dimensions[1]))) { + + 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."); + } + + Map &operator=(const Map &other) { + // Replace the memory that we point to (not a memory allocation) + new (this) Map(const_cast(other.data()), + other.rows(), + other.cols()); + return *this; + } + + operator Base() const { + return static_cast(*this); + } + + operator Base&() const { + return static_cast(*this); + } + + operator MatrixType() const { + return MatrixType(static_cast(*this)); + } +}; + + +} + +#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/gtsam.h b/gtsam.h index 7c5bc99b1..a5e24715a 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); @@ -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,6 +860,7 @@ class Cal3Bundler { void serialize() const; }; +#include class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); @@ -801,6 +890,7 @@ class CalibratedCamera { void serialize() const; }; +#include template class PinholeCamera { // Standard Constructors and Named Constructors @@ -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(); @@ -868,7 +959,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; @@ -882,6 +973,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +#include class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -959,7 +1051,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 +1073,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 +1125,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 +1177,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 @@ -1377,11 +1468,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 +1512,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 +1588,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 +1635,7 @@ virtual class GaussianBayesTree { gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; +#include class Errors { //Constructors Errors(); @@ -1553,6 +1646,7 @@ class Errors { bool equals(const gtsam::Errors& expected, double tol) const; }; +#include class GaussianISAM { //Constructor GaussianISAM(); @@ -1589,7 +1683,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 +1777,7 @@ class Ordering { void serialize() const; }; +#include class NonlinearFactorGraph { NonlinearFactorGraph(); NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); @@ -1693,6 +1788,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 +1797,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 +1811,7 @@ class NonlinearFactorGraph { void serialize() const; }; +#include virtual class NonlinearFactor { // Factor base class size_t size() const; @@ -1720,7 +1819,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 +1828,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 +1872,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 +1909,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 +1965,6 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { //************************************************************************* // Nonlinear optimizers //************************************************************************* - -#include #include virtual class NonlinearOptimizerParams { NonlinearOptimizerParams(); @@ -2020,6 +2029,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { void setVerbosityDL(string verbosityDL) const; }; +#include virtual class NonlinearOptimizer { gtsam::Values optimize(); gtsam::Values optimizeSafely(); @@ -2029,17 +2039,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 +2113,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 +2176,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 +2268,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 { @@ -2493,6 +2521,7 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { Matrix getAccelerometerCovariance() const; Matrix getIntegrationCovariance() const; bool getUse2ndOrderCoriolis() const; + void print(string s) const; }; #include @@ -2570,7 +2599,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 @@ -2636,7 +2664,7 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ namespace utilities { - #include + #include gtsam::KeyList createKeyList(Vector I); gtsam::KeyList createKeyList(string s, Vector I); gtsam::KeyVector createKeyVector(Vector I); @@ -2660,4 +2688,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_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/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index cc02a6ed2..fdf5e7511 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -15,6 +15,12 @@ if(WIN32) RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") endif() +if (APPLE) + set_target_properties(metis PROPERTIES + INSTALL_NAME_DIR + "${CMAKE_INSTALL_PREFIX}/lib") + endif() + install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) list(APPEND GTSAM_EXPORTED_TARGETS metis) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) 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/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/Rot3.h b/gtsam/geometry/Rot3.h index bb0278953..58c150670 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); 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..99c45ff34 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -115,7 +115,6 @@ public: */ Eigen::Matrix preintMeasCov_; - PreintegratedCombinedMeasurements() {} friend class CombinedImuFactor; @@ -123,6 +122,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 diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 532abdac0..55f043dd3 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 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/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/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 e6aab45da..f3fd49fa7 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -95,7 +95,7 @@ namespace gtsam { 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_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/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/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/wrap/Argument.cpp b/wrap/Argument.cpp index 01da3a756..913c11284 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -17,6 +17,7 @@ **/ #include "Argument.h" +#include "Class.h" #include @@ -62,20 +63,13 @@ 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 = !Argument::isScalar(); + bool isNotScalar = !type.isScalar(); // We cannot handle scalar non const references if (!isNotScalar && is_ref && !is_const) { @@ -110,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; @@ -160,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; } @@ -189,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/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..2637275d1 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 __str__(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..899ac3541 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; @@ -84,6 +86,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 +117,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 +165,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..8b6b199a2 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -48,6 +48,10 @@ struct ReturnValue { 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 +71,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..ec69ad081 --- /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 __str__(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