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/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/gtsam.h b/gtsam.h index 7c5bc99b1..ac4e80020 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(); @@ -662,7 +749,6 @@ virtual class Cal3DS2_Base { // 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 +840,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 +857,7 @@ class Cal3Bundler { void serialize() const; }; +#include class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); @@ -801,6 +887,7 @@ class CalibratedCamera { void serialize() const; }; +#include template class PinholeCamera { // Standard Constructors and Named Constructors @@ -832,12 +919,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 +956,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 +970,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +#include class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -959,7 +1048,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 +1070,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 +1122,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 +1174,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 +1465,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 +1509,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 +1585,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 +1632,7 @@ virtual class GaussianBayesTree { gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; +#include class Errors { //Constructors Errors(); @@ -1553,6 +1643,7 @@ class Errors { bool equals(const gtsam::Errors& expected, double tol) const; }; +#include class GaussianISAM { //Constructor GaussianISAM(); @@ -1589,7 +1680,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 +1774,7 @@ class Ordering { void serialize() const; }; +#include class NonlinearFactorGraph { NonlinearFactorGraph(); NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); @@ -1693,6 +1785,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 +1794,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 +1808,7 @@ class NonlinearFactorGraph { void serialize() const; }; +#include virtual class NonlinearFactor { // Factor base class size_t size() const; @@ -1720,7 +1816,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 +1825,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 +1869,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 +1906,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 +1962,6 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { //************************************************************************* // Nonlinear optimizers //************************************************************************* - -#include #include virtual class NonlinearOptimizerParams { NonlinearOptimizerParams(); @@ -2020,6 +2026,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { void setVerbosityDL(string verbosityDL) const; }; +#include virtual class NonlinearOptimizer { gtsam::Values optimize(); gtsam::Values optimizeSafely(); @@ -2029,17 +2036,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 +2110,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 +2173,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 +2265,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 +2518,7 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { Matrix getAccelerometerCovariance() const; Matrix getIntegrationCovariance() const; bool getUse2ndOrderCoriolis() const; + void print(string s) const; }; #include @@ -2570,7 +2596,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 +2661,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 +2685,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/CMakeLists.txt b/gtsam/CMakeLists.txt index 8c1d8bb43..c159db250 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 @@ -138,7 +137,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 +164,4 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Wrap wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") endif () + 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