12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into fix/msvc2017

release/4.3a0
Frank Dellaert 2018-10-22 19:11:21 -04:00
parent bf0c9dccaa c6d9baf3ce
commit f8793f49ff
270 changed files with 16864 additions and 3297 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.gitignore vendored
View File

@ -9,3 +9,11 @@
*.txt.user.6d59f0c
/python-build/
*.pydevproject
cython/venv
cython/gtsam.cpp
cython/gtsam.cpython-35m-darwin.so
cython/gtsam.pyx
cython/gtsam.so
cython/gtsam_wrapper.pxd
.vscode
.env

38
CMakeLists.txt
View File

@ -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")
@ -365,6 +370,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)
@ -384,10 +394,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")
@ -515,6 +535,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

6
LICENSE
View File

@ -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)

7
README.md
View File

@ -62,11 +62,14 @@ If you are using the factor in academic work, please cite the publications above
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions,
which support (superfast) automatic differentiation,
can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.

15
bitbucket-pipelines.yml Normal file
View File

@ -0,0 +1,15 @@
# Built from sample configuration for C++ – Make.
# Check https://confluence.atlassian.com/x/5Q4SMw for more examples.
# -----
# Our custom docker image from Docker Hub as the build environment.
image: dellaert/ubuntu-boost-tbb-eigen3:bionic
pipelines:
default:
- step:
script: # Modify the commands below to build your repository.
- mkdir build
- cd build
- cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF ..
- make -j2
- make -j2 check

3
cmake/CMakeLists.txt
View File

@ -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")

76
cmake/FindCython.cmake Normal file
View File

@ -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
)

18
cmake/FindMKL.cmake
View File

@ -83,10 +83,20 @@ FIND_PATH(MKL_FFTW_INCLUDE_DIR
NO_DEFAULT_PATH
)
IF(WIN32)
IF(WIN32 AND MKL_ROOT_DIR)
SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}")
IF (MKL_INCLUDE_DIR MATCHES "10.")
IF(MKL_INCLUDE_DIR MATCHES "2017" OR MKL_INCLUDE_DIR MATCHES "2018")
IF(CMAKE_CL_64)
SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_lapack95_lp64 mkl_blas95_lp64)
ELSE()
SET(MKL_LIBS mkl_core mkl_intel_s mkl_lapack95 mkl_blas95)
ENDIF()
IF(TBB_FOUND AND GTSAM_WITH_TBB)
SET(MKL_LIBS ${MKL_LIBS} mkl_tbb_thread)
ELSE()
SET(MKL_LIBS ${MKL_LIBS} mkl_intel_thread libiomp5md)
ENDIF()
ELSEIF(MKL_INCLUDE_DIR MATCHES "10.")
IF(CMAKE_CL_64)
SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64)
ELSE()
@ -115,7 +125,7 @@ IF(WIN32)
ENDIF()
ENDFOREACH()
SET(MKL_FOUND ON)
ELSE() # UNIX and macOS
ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
FIND_LIBRARY(MKL_CORE_LIBRARY
mkl_core
PATHS

4
cmake/FindTBB.cmake
View File

@ -82,6 +82,10 @@ if (WIN32)
set(_TBB_COMPILER "vc11")
set(TBB_COMPILER "vc11")
endif(MSVC11)
if(MSVC14)
set(_TBB_COMPILER "vc14")
set(TBB_COMPILER "vc14")
endif(MSVC14)
# Todo: add other Windows compilers such as ICL.
if(TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})

246
cmake/GtsamCythonWrap.cmake Normal file
View File

@ -0,0 +1,246 @@
# Check Cython version, need to be >=0.25.2
# Unset these cached variables to avoid surprises when the python/cython
# in the current environment are different from the cached!
unset(PYTHON_EXECUTABLE CACHE)
unset(CYTHON_EXECUTABLE CACHE)
find_package(Cython 0.25.2 REQUIRED)
# User-friendly Cython wrapping and installing function.
# Builds a Cython module from the provided interface_header.
# For example, for the interface header gtsam.h,
# this will build the wrap module 'gtsam'.
#
# Arguments:
#
# interface_header: The relative path to the wrapper interface definition file.
# extra_imports: extra header to import in the Cython pxd file.
# For example, to use Cython gtsam.pxd in your own module,
# use "from gtsam cimport *"
# install_path: destination to install the library
# libs: libraries to link with
# dependencies: Dependencies which need to be built before the wrapper
function(wrap_and_install_library_cython interface_header extra_imports install_path libs dependencies)
# Paths for generated files
get_filename_component(module_name "${interface_header}" NAME_WE)
set(generated_files_path "${PROJECT_BINARY_DIR}/cython/${module_name}")
wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}")
install_cython_wrapped_library("${interface_header}" "${generated_files_path}" "${install_path}")
endfunction()
function(set_up_required_cython_packages)
# Set up building of cython module
find_package(PythonLibs 2.7 REQUIRED)
include_directories(${PYTHON_INCLUDE_DIRS})
find_package(NumPy REQUIRED)
include_directories(${NUMPY_INCLUDE_DIRS})
endfunction()
# Convert pyx to cpp by executing cython
# This is the first step to compile cython from the command line
# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
#
# Arguments:
# - target: The specified target for this step
# - pyx_file: The input pyx_file in full *absolute* path
# - generated_cpp: The output cpp file in full absolute path
# - include_dirs: Directories to include when executing cython
function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
foreach(dir ${include_dirs})
set(includes_for_cython ${includes_for_cython} -I ${dir})
endforeach()
add_custom_command(
OUTPUT ${generated_cpp}
COMMAND
${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
VERBATIM)
add_custom_target(${target} ALL DEPENDS ${generated_cpp})
endfunction()
# Build the cpp file generated by converting pyx using cython
# This is the second step to compile cython from the command line
# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
#
# Arguments:
# - target: The specified target for this step
# - cpp_file: The input cpp_file in full *absolute* path
# - output_lib_we: The output lib filename only (without extension)
# - output_dir: The output directory
function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
add_library(${target} MODULE ${cpp_file})
if(APPLE)
set(link_flags "-undefined dynamic_lookup")
endif()
set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "${link_flags}"
OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir})
endfunction()
# Cythonize a pyx from the command line as described at
# http://cython.readthedocs.io/en/latest/src/reference/compilation.html
# Arguments:
# - target: The specified target
# - pyx_file: The input pyx_file in full *absolute* path
# - output_lib_we: The output lib filename only (without extension)
# - output_dir: The output directory
# - include_dirs: Directories to include when executing cython
# - libs: Libraries to link with
# - interface_header: For dependency. Any update in interface header will re-trigger cythonize
function(cythonize target pyx_file output_lib_we output_dir include_dirs libs interface_header dependencies)
get_filename_component(pyx_path "${pyx_file}" DIRECTORY)
get_filename_component(pyx_name "${pyx_file}" NAME_WE)
set(generated_cpp "${output_dir}/${pyx_name}.cpp")
set_up_required_cython_packages()
pyx_to_cpp(${target}_pyx2cpp ${pyx_file} ${generated_cpp} "${include_dirs}")
# Late dependency injection, to make sure this gets called whenever the interface header is updated
# See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
if (NOT "${dependencies}" STREQUAL "")
add_dependencies(${target}_pyx2cpp "${dependencies}")
endif()
build_cythonized_cpp(${target} ${generated_cpp} ${output_lib_we} ${output_dir})
if (NOT "${libs}" STREQUAL "")
target_link_libraries(${target} "${libs}")
endif()
add_dependencies(${target} ${target}_pyx2cpp)
endfunction()
# Internal function that wraps a library and compiles the wrapper
function(wrap_library_cython interface_header generated_files_path extra_imports libs dependencies)
# Wrap codegen interface
# Extract module path and name from interface header file name
# wrap requires interfacePath to be *absolute*
get_filename_component(interface_header "${interface_header}" ABSOLUTE)
get_filename_component(module_path "${interface_header}" PATH)
get_filename_component(module_name "${interface_header}" NAME_WE)
# Wrap module to Cython pyx
message(STATUS "Cython wrapper generating ${module_name}.pyx")
set(generated_pyx "${generated_files_path}/${module_name}.pyx")
file(MAKE_DIRECTORY "${generated_files_path}")
add_custom_command(
OUTPUT ${generated_pyx}
DEPENDS ${interface_header} wrap
COMMAND
wrap --cython ${module_path} ${module_name} ${generated_files_path} "${extra_imports}"
VERBATIM
WORKING_DIRECTORY ${generated_files_path}/../)
add_custom_target(cython_wrap_${module_name}_pyx ALL DEPENDS ${generated_pyx})
if(NOT "${dependencies}" STREQUAL "")
add_dependencies(cython_wrap_${module_name}_pyx ${dependencies})
endif()
message(STATUS "Cythonize and build ${module_name}.pyx")
get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
cythonize(cythonize_${module_name} ${generated_pyx} ${module_name}
${generated_files_path} "${include_dirs}" "${libs}" ${interface_header} cython_wrap_${module_name}_pyx)
# distclean
add_custom_target(wrap_${module_name}_cython_distclean
COMMAND cmake -E remove_directory ${generated_files_path})
endfunction()
# Internal function that installs a wrap toolbox
function(install_cython_wrapped_library interface_header generated_files_path install_path)
get_filename_component(module_name "${interface_header}" NAME_WE)
# NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name
# here prevents creating the top-level module name directory in the destination.
message(STATUS "Installing Cython Toolbox to ${install_path}") #${GTSAM_CYTHON_INSTALL_PATH}")
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
get_filename_component(location "${install_path}" PATH)
get_filename_component(name "${install_path}" NAME)
install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}"
CONFIGURATIONS "${build_type}"
PATTERN "build" EXCLUDE
PATTERN "CMakeFiles" EXCLUDE
PATTERN "Makefile" EXCLUDE
PATTERN "*.cmake" EXCLUDE
PATTERN "*.cpp" EXCLUDE
PATTERN "*.py" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${generated_files_path}/" DESTINATION ${install_path}
PATTERN "build" EXCLUDE
PATTERN "CMakeFiles" EXCLUDE
PATTERN "Makefile" EXCLUDE
PATTERN "*.cmake" EXCLUDE
PATTERN "*.cpp" EXCLUDE
PATTERN "*.py" EXCLUDE)
endif()
endfunction()
# Helper function to install Cython scripts and handle multiple build types where the scripts
# should be installed to all build type toolboxes
#
# Arguments:
# source_directory: The source directory to be installed. "The last component of each directory
# name is appended to the destination directory but a trailing slash may be
# used to avoid this because it leaves the last component empty."
# (https://cmake.org/cmake/help/v3.3/command/install.html?highlight=install#installing-directories)
# dest_directory: The destination directory to install to.
# patterns: list of file patterns to install
function(install_cython_scripts source_directory dest_directory patterns)
set(patterns_args "")
set(exclude_patterns "")
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
get_filename_component(location "${dest_directory}" PATH)
get_filename_component(name "${dest_directory}" NAME)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}"
FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endif()
endfunction()
# Helper function to install specific files and handle multiple build types where the scripts
# should be installed to all build type toolboxes
#
# Arguments:
# source_files: The source files to be installed.
# dest_directory: The destination directory to install to.
function(install_cython_files source_files dest_directory)
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
get_filename_component(location "${dest_directory}" PATH)
get_filename_component(name "${dest_directory}" NAME)
install(FILES "${source_files}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}")
endforeach()
else()
install(FILES "${source_files}" DESTINATION "${dest_directory}")
endif()
endfunction()

26
cmake/GtsamMatlabWrap.cmake
View File

@ -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()

40
cython/CMakeLists.txt Normal file
View File

@ -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 ()

154
cython/README.md Normal file
View File

@ -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 <gtsam_folder>/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: <your CMAKE_INSTALL_PREFIX>/cython
- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH:
```bash
export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
```
UNIT TESTS
==========
The Cython toolbox also has a small set of unit tests located in the
test directory. To run them:
```bash
cd <your GTSAM_CYTHON_INSTALL_PATH>
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 <innerNamespace>_ 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<size_t> for keys(), which can't be casted to FastVector<Key>
- 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 <package> * ": 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<Key>, to FastVector<size_t>.
✘ 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<size_t> 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?

2
cython/gtsam/__init__.py.in Normal file
View File

@ -0,0 +1,2 @@
from gtsam import *
${GTSAM_UNSTABLE_IMPORT}

176
cython/gtsam/examples/ImuFactorExample2.py Normal file
View File

@ -0,0 +1,176 @@
"""
iSAM2 example with ImuFactor.
Author: Robert Truax (C++), Frank Dellaert (Python)
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol(ord('x'), key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)
def ISAM2_plot(values, fignum=0):
"""Plot poses."""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
i = 0
min_bounds = 0, 0, 0
max_bounds = 0, 0, 0
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
position = pose_i.translation().vector()
min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
# max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
i += 1
# draw
axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
plt.pause(1)
# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
g = 9.81
n_gravity = vector3(0, 0, -g)
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
I = np.eye(3)
PARAMS.setAccelerometerCovariance(I * 0.1)
PARAMS.setGyroscopeCovariance(I * 0.1)
PARAMS.setIntegrationCovariance(I * 0.1)
PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20))
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t # simulation time
if i == 0: # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))
elif i >= 2: # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))
if i > 0:
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = gtsam.BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()
if __name__ == '__main__':
IMU_example()

52
cython/gtsam/examples/OdometryExample.py Normal file
View File

@ -0,0 +1,52 @@
"""
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
Simple robot motion example, with prior and two odometry measurements
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import numpy as np
import gtsam
# Create noise models
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
# For simplicity, we will use the same noise model for each odometry factor
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
print("\nInitial Estimate:\n{}".format(initial))
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))

80
cython/gtsam/examples/PlanarSLAMExample.py Normal file
View File

@ -0,0 +1,80 @@
"""
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
Simple robotics example using odometry measurements and bearing-range (laser) measurements
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import numpy as np
import gtsam
# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = gtsam.symbol(ord('x'), 1)
X2 = gtsam.symbol(ord('x'), 2)
X3 = gtsam.symbol(ord('x'), 3)
L1 = gtsam.symbol(ord('l'), 4)
L2 = gtsam.symbol(ord('l'), 5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(gtsam.BetweenFactorPose2(
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(gtsam.BearingRangeFactor2D(
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
# Print graph
print("Factor Graph:\n{}".format(graph))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Print
print("Initial Estimate:\n{}".format(initial_estimate))
# Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))

87
cython/gtsam/examples/Pose2SLAMExample.py Normal file
View File

@ -0,0 +1,87 @@
"""
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
Simple robotics example using odometry measurements and bearing-range (laser) measurements
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import math
import numpy as np
import gtsam
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)
# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
# 1. Create a factor graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
# 2a. Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
# 2b. Add odometry factors
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
# 2c. Add the loop closure constraint
# This factor encodes the fact that we have returned to the same pose. In real
# systems, these constraints may be identified in many ways, such as appearance-based
# techniques with camera images. We will use another Between Factor to enforce this constraint:
graph.add(gtsam.BetweenFactorPose2(
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph)) # print
# 3. Create the data structure to hold the initial_estimate estimate to the
# solution. For illustrative purposes, these have been deliberately set to incorrect values
initial_estimate = gtsam.Values()
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))
print("\nInitial Estimate:\n{}".format(initial_estimate)) # print
# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
# The optimizer accepts an optional set of configuration parameters,
# controlling things like convergence criteria, the type of linear
# system solver to use, and the amount of information displayed during
# optimization. We will set a few parameters as a demonstration.
parameters = gtsam.GaussNewtonParams()
# Stop iterating once the change in error between steps is less than this value
parameters.setRelativeErrorTol(1e-5)
# Do not perform more than N iteration steps
parameters.setMaxIterations(100)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)
# ... and optimize
result = optimizer.optimize()
print("Final Result:\n{}".format(result))
# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 6):
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))

4
cython/gtsam/examples/README.md Normal file
View File

@ -0,0 +1,4 @@
These examples are almost identical to the old handwritten python wrapper
examples. However, there are just some slight name changes, for example
noiseModel.Diagonal becomes noiseModel_Diagonal etc...
Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0))

38
cython/gtsam/examples/SFMdata.py Normal file
View File

@ -0,0 +1,38 @@
"""
A structure-from-motion example with landmarks
- The landmarks form a 10 meter cube
- The robot rotates around the landmarks, always facing towards the cube
"""
# pylint: disable=invalid-name, E1101
import numpy as np
import gtsam
def createPoints():
# Create the set of ground-truth landmarks
points = [gtsam.Point3(10.0, 10.0, 10.0),
gtsam.Point3(-10.0, 10.0, 10.0),
gtsam.Point3(-10.0, -10.0, 10.0),
gtsam.Point3(10.0, -10.0, 10.0),
gtsam.Point3(10.0, 10.0, -10.0),
gtsam.Point3(-10.0, 10.0, -10.0),
gtsam.Point3(-10.0, -10.0, -10.0),
gtsam.Point3(10.0, -10.0, -10.0)]
return points
def createPoses(K):
# Create the set of ground-truth poses
radius = 30.0
angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta), 0.0)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses

163
cython/gtsam/examples/VisualISAM2Example.py Normal file
View File

@ -0,0 +1,163 @@
"""
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
An example of running visual SLAM using iSAM2.
Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
from gtsam.examples import SFMdata
def X(i):
"""Create key for pose i."""
return int(gtsam.symbol(ord('x'), i))
def L(j):
"""Create key for landmark j."""
return int(gtsam.symbol(ord('l'), j))
def visual_ISAM2_plot(result):
"""
VisualISAMPlot plots current state of ISAM2 object
Author: Ellon Paiva
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
"""
# Declare an id for the figure
fignum = 0
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
# gtsam.plot_3d_points(result, [], marginals)
gtsam_plot.plot_3d_points(fignum, result, 'rx')
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
i += 1
# draw
axes.set_xlim3d(-40, 40)
axes.set_ylim3d(-40, 40)
axes.set_zlim3d(-40, 40)
plt.pause(1)
def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
# to maintain proper linearization and efficient variable ordering, iSAM2
# performs partial relinearization/reordering at each step. A parameter
# structure is available that allows the user to set various properties, such
# as the relinearization threshold and type of linear solver. For this
# example, we we set the relinearization threshold small so the iSAM2 result
# will approach the batch result.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.01)
parameters.setRelinearizeSkip(1)
isam = gtsam.ISAM2(parameters)
# Create a Factor Graph and Values to hold the new data
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
# Add factors for each landmark observation
for j, point in enumerate(points):
camera = gtsam.PinholeCameraCal3_S2(pose, K)
measurement = camera.project(point)
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(
measurement, measurement_noise, X(i), L(j), K))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
initial_estimate.insert(X(i), pose.compose(gtsam.Pose3(
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
# If this is the first iteration, add a prior on the first pose to set the
# coordinate frame and a prior on the first landmark to set the scale.
# Also, as iSAM solves incrementally, we must wait until each is observed
# at least twice before adding it to iSAM.
if i == 0:
# Add a prior on pose x0
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
[0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
graph.push_back(gtsam.PriorFactorPoint3(
L(0), points[0], point_noise)) # add directly to graph
# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initial_estimate.insert(L(j), gtsam.Point3(
point.x()-0.25, point.y()+0.20, point.z()+0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
# If accuracy is desired at the expense of time, update(*) can be called additional
# times to perform multiple optimizer iterations every step.
isam.update()
current_estimate = isam.calculateEstimate()
print("****************************************************")
print("Frame", i, ":")
for j in range(i + 1):
print(X(j), ":", current_estimate.atPose3(X(j)))
for j in range(len(points)):
print(L(j), ":", current_estimate.atPoint3(L(j)))
visual_ISAM2_plot(current_estimate)
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
plt.ioff()
plt.show()
if __name__ == '__main__':
visual_ISAM2_example()

0
cython/gtsam/examples/__init__.py Normal file
View File

0
cython/gtsam/tests/__init__.py Normal file
View File

112
cython/gtsam/tests/experiments.py Normal file
View File

@ -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)

772
cython/gtsam/tests/gtsam_test.h Normal file
View File

@ -0,0 +1,772 @@
namespace gtsam {
#include <gtsam/inference/Key.h>
typedef size_t Key;
#include <gtsam/base/FastVector.h>
template<T> 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<gtsam::Key> KeyVector;
//*************************************************************************
// geometry
//*************************************************************************
#include <gtsam/geometry/Point2.h>
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 <gtsam/geometry/Point3.h>
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 <gtsam/geometry/Rot2.h>
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 <gtsam/geometry/Rot3.h>
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 <gtsam/geometry/Pose2.h>
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 <gtsam/geometry/Pose3.h>
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 <gtsam/linear/NoiseModel.h>
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 <gtsam/linear/Sampler.h>
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 <gtsam/linear/VectorValues.h>
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 <gtsam/linear/GaussianFactor.h>
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<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
};
#include <gtsam/linear/JacobianFactor.h>
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<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
//pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> 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 <gtsam/linear/HessianFactor.h>
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 <gtsam/nonlinear/Values.h>
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 <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
// gtsam::Rot3, gtsam::Pose3}>
// void insert(size_t j, const T& t);
// template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
// gtsam::Rot3, gtsam::Pose3}>
// 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 = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, Vector, Matrix}>
T at(size_t j);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
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 <gtsam/nonlinear/NonlinearFactorGraph.h>
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<gtsam::Key,int>& constraints) const;
//gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
gtsam::NonlinearFactorGraph clone() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
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 <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
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 <gtsam/slam/BetweenFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
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 <gtsam/inference/Symbol.h>
size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
#include <gtsam/inference/Key.h>
// Default keyformatter
void PrintKeyVector(const gtsam::KeyVector& keys);
void PrintKeyVector(const gtsam::KeyVector& keys, string s);
#include <gtsam/nonlinear/NonlinearOptimizer.h>
bool checkConvergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError);
#include <gtsam/slam/dataset.h>
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
//*************************************************************************
// Utilities
//*************************************************************************
namespace utilities {
#include <gtsam/nonlinear/utilities.h>
// 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 <gtsam/nonlinear/utilities.h>
class RedirectCout {
RedirectCout();
string str();
};
} //\namespace gtsam

22
cython/gtsam/tests/test_Cal3Unified.py Normal file
View File

@ -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()

78
cython/gtsam/tests/test_JacobianFactor.py Normal file
View File

@ -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()

69
cython/gtsam/tests/test_KalmanFilter.py Normal file
View File

@ -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()

50
cython/gtsam/tests/test_LocalizationExample.py Normal file
View File

@ -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()

45
cython/gtsam/tests/test_OdometryExample.py Normal file
View File

@ -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()

64
cython/gtsam/tests/test_PlanarSLAMExample.py Normal file
View File

@ -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()

62
cython/gtsam/tests/test_Pose2SLAMExample.py Normal file
View File

@ -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()

44
cython/gtsam/tests/test_Pose3.py Normal file
View File

@ -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()

46
cython/gtsam/tests/test_Pose3SLAMExample.py Normal file
View File

@ -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()

25
cython/gtsam/tests/test_PriorFactor.py Normal file
View File

@ -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()

69
cython/gtsam/tests/test_SFMExample.py Normal file
View File

@ -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()

36
cython/gtsam/tests/test_Scenario.py Normal file
View File

@ -0,0 +1,36 @@
import math
import unittest
import numpy as np
import gtsam
class TestScenario(unittest.TestCase):
def setUp(self):
pass
def test_loop(self):
# Forward velocity 2m/s
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
v = 2
w = math.radians(6)
W = np.array([0, -w, 0])
V = np.array([v, 0, 0])
scenario = gtsam.ConstantTwistScenario(W, V)
T = 30
np.testing.assert_almost_equal(W, scenario.omega_b(T))
np.testing.assert_almost_equal(V, scenario.velocity_b(T))
np.testing.assert_almost_equal(
np.cross(W, V), scenario.acceleration_b(T))
# R = v/w, so test if loop crests at 2*R
R = v / w
T30 = scenario.pose(T)
np.testing.assert_almost_equal(
np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
if __name__ == '__main__':
unittest.main()

32
cython/gtsam/tests/test_SimpleCamera.py Normal file
View File

@ -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()

69
cython/gtsam/tests/test_StereoVOExample.py Normal file
View File

@ -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()

69
cython/gtsam/tests/test_Values.py Normal file
View File

@ -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()

42
cython/gtsam/tests/test_VisualISAMExample.py Normal file
View File

@ -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()

0
cython/gtsam/utils/__init__.py Normal file
View File

38
cython/gtsam/utils/circlePose3.py Normal file
View File

@ -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

77
cython/gtsam/utils/plot.py Normal file
View File

@ -0,0 +1,77 @@
"""Various plotting utlities."""
import numpy as np
import matplotlib.pyplot as plt
def plot_point3_on_axes(axes, point, linespec):
"""Plot a 3D point on given axis 'axes' with given 'linespec'."""
axes.plot([point.x()], [point.y()], [point.z()], linespec)
def plot_point3(fignum, point, linespec):
"""Plot a 3D point on given figure with given 'linespec'."""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_point3_on_axes(axes, point, linespec)
def plot_3d_points(fignum, values, linespec, marginals=None):
"""
Plots the Point3s in 'values', with optional covariances.
Finds all the Point3 objects in the given Values object and plots them.
If a Marginals object is given, this function will also plot marginal
covariance ellipses for each point.
"""
keys = values.keys()
# Plot points and covariance matrices
for i in range(keys.size()):
try:
p = values.atPoint3(keys.at(i))
# if haveMarginals
# P = marginals.marginalCovariance(key);
# gtsam.plot_point3(p, linespec, P);
# else
plot_point3(fignum, p, linespec)
except RuntimeError:
continue
# I guess it's not a Point3
def plot_pose3_on_axes(axes, pose, axis_length=0.1):
"""Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
origin = np.array([t.x(), t.y(), t.z()])
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')
y_axis = origin + gRp[:, 1] * axis_length
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')
z_axis = origin + gRp[:, 2] * axis_length
line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
# plot the covariance
# TODO (dellaert): make this work
# if (nargin>2) && (~isempty(P))
# pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
# gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
# gtsam.covarianceEllipse3D(origin,gPp);
# end
def plot_pose3(fignum, pose, axis_length=0.1):
"""Plot a 3D pose on given figure with given 'axis_length'."""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_pose3_on_axes(axes, pose, axis_length)

117
cython/gtsam/utils/visual_data_generator.py Normal file
View File

@ -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

131
cython/gtsam/utils/visual_isam.py Normal file
View File

@ -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

38
cython/gtsam_eigency/CMakeLists.txt Normal file
View File

@ -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)

20
cython/gtsam_eigency/LICENSE.txt Normal file
View File

@ -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.

13
cython/gtsam_eigency/__init__.py.in Normal file
View File

@ -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

62
cython/gtsam_eigency/conversions.pxd Normal file
View File

@ -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)

327
cython/gtsam_eigency/conversions.pyx Normal file
View File

@ -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 = <double[:rows,:cols]>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 = <double[:rows:1,:cols]>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 = <double[:rows,:cols]>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 = <double[:rows:1,:cols]>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 = <float[:rows,:cols]>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 = <float[:rows:1,:cols]>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 = <float[:rows,:cols]>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 = <float[:rows:1,:cols]>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 = <long[:rows,:cols]>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 = <long[:rows:1,:cols]>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 = <long[:rows,:cols]>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 = <long[:rows:1,:cols]>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 = <unsigned long[:rows,:cols]>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 = <unsigned long[:rows:1,:cols]>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 = <unsigned long[:rows,:cols]>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 = <unsigned long[:rows:1,:cols]>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 = <int[:rows,:cols]>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 = <int[:rows:1,:cols]>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 = <int[:rows,:cols]>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 = <int[:rows:1,:cols]>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 = <unsigned int[:rows,:cols]>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 = <unsigned int[:rows:1,:cols]>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 = <unsigned int[:rows,:cols]>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 = <unsigned int[:rows:1,:cols]>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 = <short[:rows,:cols]>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 = <short[:rows:1,:cols]>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 = <short[:rows,:cols]>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 = <short[:rows:1,:cols]>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 = <unsigned short[:rows,:cols]>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 = <unsigned short[:rows:1,:cols]>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 = <unsigned short[:rows,:cols]>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 = <unsigned short[:rows:1,:cols]>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 = <signed char[:rows,:cols]>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 = <signed char[:rows:1,:cols]>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 = <signed char[:rows,:cols]>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 = <signed char[:rows:1,:cols]>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 = <unsigned char[:rows,:cols]>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 = <unsigned char[:rows:1,:cols]>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 = <unsigned char[:rows,:cols]>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 = <unsigned char[:rows:1,:cols]>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 = <np.complex128_t[:rows,:cols]>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 = <np.complex128_t[:rows:1,:cols]>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 = <np.complex128_t[:rows,:cols]>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 = <np.complex128_t[:rows:1,:cols]>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 = <np.complex64_t[:rows,:cols]>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 = <np.complex64_t[:rows:1,:cols]>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 = <np.complex64_t[:rows,:cols]>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 = <np.complex64_t[:rows:1,:cols]>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]))

917
cython/gtsam_eigency/core.pxd Normal file
View File

@ -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

1
cython/gtsam_eigency/core.pyx Normal file
View File

@ -0,0 +1 @@

504
cython/gtsam_eigency/eigency_cpp.h Normal file
View File

@ -0,0 +1,504 @@
#include <Eigen/Dense>
#include <iostream>
#include <stdexcept>
#include <complex>
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<typename Scalar>
inline PyArrayObject *_ndarray_view(Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0);
template<typename Scalar>
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>(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<double>(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>(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<float>(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>(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<long>(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>(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<unsigned long>(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>(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<int>(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>(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<unsigned int>(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>(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<short>(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>(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<unsigned short>(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>(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<signed char>(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>(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<unsigned char>(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<double> >(std::complex<double> *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<std::complex<double> >(const std::complex<double> *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<float> >(std::complex<float> *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<std::complex<float> >(const std::complex<float> *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 <typename Derived>
inline PyArrayObject *ndarray(Eigen::PlainObjectBase<Derived> &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 <typename Derived>
inline PyArrayObject *ndarray(Eigen::PlainObjectBase<Derived> &&m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
#endif
template <typename Derived>
inline PyArrayObject *ndarray(const Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived>
inline PyArrayObject *ndarray_view(Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived>
inline PyArrayObject *ndarray_view(const Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived>
inline PyArrayObject *ndarray_copy(const Eigen::PlainObjectBase<Derived> &m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor);
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray(Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray(const Eigen::Map<Derived, MapOptions, Stride> &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<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray_view(Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray_view(const Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_view(const_cast<typename Derived::Scalar*>(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename Derived, int MapOptions, typename Stride>
inline PyArrayObject *ndarray_copy(const Eigen::Map<Derived, MapOptions, Stride> &m) {
import_gtsam_eigency__conversions();
return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride());
}
template <typename MatrixType,
int _MapOptions = Eigen::Unaligned,
typename _StrideType=Eigen::Stride<0,0> >
class MapBase: public Eigen::Map<MatrixType, _MapOptions, _StrideType> {
public:
typedef Eigen::Map<MatrixType, _MapOptions, _StrideType> Base;
typedef typename Base::Scalar Scalar;
MapBase(Scalar* data,
long rows,
long cols,
_StrideType stride=_StrideType())
: Base(data,
// If both dimensions are dynamic or dimensions match, accept dimensions as they are
((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) ||
(Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols))
? rows
// otherwise, test if swapping them makes them fit
: ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows)
? cols
: rows),
((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) ||
(Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols))
? cols
: ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows)
? rows
: cols),
stride
) {}
MapBase &operator=(const MatrixType &other) {
Base::operator=(other);
return *this;
}
virtual ~MapBase() { }
};
template <template<class,int,int,int,int,int> class EigencyDenseBase,
typename Scalar,
int _Rows, int _Cols,
int _Options = Eigen::AutoAlign |
#if defined(__GNUC__) && __GNUC__==3 && __GNUC_MINOR__==4
// workaround a bug in at least gcc 3.4.6
// the innermost ?: ternary operator is misparsed. We write it slightly
// differently and this makes gcc 3.4.6 happy, but it's ugly.
// The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined
// (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor)
( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19
#if EIGEN_VERSION_AT_LEAST(3,2,90)
: !(_Cols==1 && _Rows!=1) ? EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
#else
: !(_Cols==1 && _Rows!=1) ? Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION
#endif
: ColMajor ),
#else
( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor
: (_Cols==1 && _Rows!=1) ? Eigen::ColMajor
// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19
#if EIGEN_VERSION_AT_LEAST(3,2,90)
: EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
#else
: Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
#endif
#endif
int _MapOptions = Eigen::Unaligned,
int _StrideOuter=0, int _StrideInner=0,
int _MaxRows = _Rows,
int _MaxCols = _Cols>
class FlattenedMap: public MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > {
public:
typedef MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
FlattenedMap()
: Base(NULL, 0, 0),
object_(NULL) {}
FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
: Base(data, rows, cols,
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
object_(NULL) {
}
FlattenedMap(PyArrayObject *object)
: Base((Scalar *)((PyArrayObject*)object)->data,
// : Base(_from_numpy<Scalar>((PyArrayObject*)object),
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
_StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])),
object_(object) {
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
Py_XINCREF(object_);
}
FlattenedMap &operator=(const FlattenedMap &other) {
if (other.object_) {
new (this) FlattenedMap(other.object_);
} else {
// Replace the memory that we point to (not a memory allocation)
new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
other.rows(),
other.cols(),
other.outerStride(),
other.innerStride());
}
return *this;
}
operator Base() const {
return static_cast<Base>(*this);
}
operator Base&() const {
return static_cast<Base&>(*this);
}
operator EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
return EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
}
virtual ~FlattenedMap() {
Py_XDECREF(object_);
}
private:
PyArrayObject * const object_;
};
template <typename MatrixType>
class Map: public MapBase<MatrixType> {
public:
typedef MapBase<MatrixType> Base;
typedef typename MatrixType::Scalar Scalar;
enum {
RowsAtCompileTime = Base::Base::RowsAtCompileTime,
ColsAtCompileTime = Base::Base::ColsAtCompileTime
};
Map()
: Base(NULL,
(RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime,
(ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
object_(NULL) {
}
Map(Scalar *data, long rows, long cols)
: Base(data, rows, cols),
object_(NULL) {}
Map(PyArrayObject *object)
: Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data,
// ROW: If array is in row-major order, transpose (see README)
(PyObject*)object == Py_None? 0 :
(!PyArray_IS_F_CONTIGUOUS(object)
? ((object->nd == 1)
? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector)
: object->dimensions[1])
: object->dimensions[0]),
// COLUMN: If array is in row-major order: transpose (see README)
(PyObject*)object == Py_None? 0 :
(!PyArray_IS_F_CONTIGUOUS(object)
? object->dimensions[0]
: ((object->nd == 1)
? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector)
: object->dimensions[1]))),
object_(object) {
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
Py_XINCREF(object_);
}
Map &operator=(const Map &other) {
if (other.object_) {
new (this) Map(other.object_);
} else {
// Replace the memory that we point to (not a memory allocation)
new (this) Map(const_cast<Scalar*>(other.data()),
other.rows(),
other.cols());
}
return *this;
}
Map &operator=(const MatrixType &other) {
MapBase<MatrixType>::operator=(other);
return *this;
}
operator Base() const {
return static_cast<Base>(*this);
}
operator Base&() const {
return static_cast<Base&>(*this);
}
operator MatrixType() const {
return MatrixType(static_cast<Base>(*this));
}
virtual ~Map() {
Py_XDECREF(object_);
}
private:
PyArrayObject * const object_;
};
}
#endif

3
cython/requirements.txt Normal file
View File

@ -0,0 +1,3 @@
Cython>=0.25.2
backports_abc>=0.5
numpy>=1.12.0

7
doc/Code/LocalizationExample2.cpp Normal file
View File

@ -0,0 +1,7 @@
// add unary measurement factors, like GPS, on all three poses
noiseModel::Diagonal::shared_ptr unaryNoise =
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

14
doc/Code/LocalizationFactor.cpp Normal file
View File

@ -0,0 +1,14 @@
class UnaryFactor: public NoiseModelFactor1<Pose2> {
double mx_, my_; ///< X and Y measurements
public:
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const
{
if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
};

18
doc/Code/LocalizationOutput5.txt Normal file
View File

@ -0,0 +1,18 @@
Final Result:
Values with 3 values:
Value 1: (-1.5e-14, 1.3e-15, -1.4e-16)
Value 2: (2, 3.1e-16, -8.5e-17)
Value 3: (4, -6e-16, -8.2e-17)
x1 covariance:
0.0083 4.3e-19 -1.1e-18
4.3e-19 0.0094 -0.0031
-1.1e-18 -0.0031 0.0082
x2 covariance:
0.0071 2.5e-19 -3.4e-19
2.5e-19 0.0078 -0.0011
-3.4e-19 -0.0011 0.0082
x3 covariance:
0.0083 4.4e-19 1.2e-18
4.4e-19 0.0094 0.0031
1.2e-18 0.0031 0.018

15
doc/Code/OdometryExample.cpp Normal file
View File

@ -0,0 +1,15 @@
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

6
doc/Code/OdometryMarginals.cpp Normal file
View File

@ -0,0 +1,6 @@
// Query the marginals
cout.precision(2);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

9
doc/Code/OdometryOptimize.cpp Normal file
View File

@ -0,0 +1,9 @@
// create (deliberatly inaccurate) initial estimate
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

11
doc/Code/OdometryOutput1.txt Normal file
View File

@ -0,0 +1,11 @@
Factor Graph:
size: 3
factor 0: PriorFactor on 1
prior mean: (0, 0, 0)
noise model: diagonal sigmas [0.3; 0.3; 0.1];
factor 1: BetweenFactor(1,2)
measured: (2, 0, 0)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
factor 2: BetweenFactor(2,3)
measured: (2, 0, 0)
noise model: diagonal sigmas [0.2; 0.2; 0.1];

11
doc/Code/OdometryOutput2.txt Normal file
View File

@ -0,0 +1,11 @@
Initial Estimate:
Values with 3 values:
Value 1: (0.5, 0, 0.2)
Value 2: (2.3, 0.1, -0.2)
Value 3: (4.1, 0.1, 0.1)
Final Result:
Values with 3 values:
Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
Value 2: (2, 7.4e-18, -2.5e-18)
Value 3: (4, -1.8e-18, -3.1e-18)

12
doc/Code/OdometryOutput3.txt Normal file
View File

@ -0,0 +1,12 @@
x1 covariance:
0.09 1.1e-47 5.7e-33
1.1e-47 0.09 1.9e-17
5.7e-33 1.9e-17 0.01
x2 covariance:
0.13 4.7e-18 2.4e-18
4.7e-18 0.17 0.02
2.4e-18 0.02 0.02
x3 covariance:
0.17 2.7e-17 8.4e-18
2.7e-17 0.37 0.06
8.4e-18 0.06 0.03

25
doc/Code/PlanarSLAMExample.m Normal file
View File

@ -0,0 +1,25 @@
% Create graph container and add factors to it
graph = NonlinearFactorGraph;
% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);
% Add prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
% add directly to graph
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));
% Add odometry
odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
% Add bearing/range measurement factors
degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

7
doc/Code/PlanarSLAMExample.txt Normal file
View File

@ -0,0 +1,7 @@
>> result
Values with 5 values:
l1: (2, 2)
l2: (4, 2)
x1: (-1.8e-16, 5.1e-17, -1.5e-17)
x2: (2, -5.8e-16, -4.6e-16)
x3: (4, -3.1e-15, -4.6e-16)

15
doc/Code/Pose2SLAMExample-graph.m Normal file
View File

@ -0,0 +1,15 @@
%% Initialize graph, initial estimate, and odometry noise
datafile = findExampleDataFile('w100.graph');
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
[graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on pose x_0
priorMean = Pose2(0, 0, 0);
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
graph.add(PriorFactorPose2(0, priorMean, priorNoise));
%% Optimize using Levenberg-Marquardt optimization and get marginals
optimizer = LevenbergMarquardtOptimizer(graph, initial);
result = optimizer.optimizeSafely;
marginals = Marginals(graph, result);

16
doc/Code/Pose2SLAMExample.cpp Normal file
View File

@ -0,0 +1,16 @@
NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// Add odometry factors
noiseModel::Diagonal::shared_ptr model =
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));

14
doc/Code/Pose2SLAMExample.m Normal file
View File

@ -0,0 +1,14 @@
graph = NonlinearFactorGraph;
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
graph.add(PriorFactorPose2(1, Pose2(0, 0, 0), priorNoise));
%% Add odometry factors
model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactorPose2(2, 3, Pose2(2, 0, pi/2), model));
graph.add(BetweenFactorPose2(3, 4, Pose2(2, 0, pi/2), model));
graph.add(BetweenFactorPose2(4, 5, Pose2(2, 0, pi/2), model));
%% Add pose constraint
graph.add(BetweenFactorPose2(5, 2, Pose2(2, 0, pi/2), model));

12
doc/Code/Pose3SLAMExample-graph.m Normal file
View File

@ -0,0 +1,12 @@
%% Initialize graph, initial estimate, and odometry noise
datafile = findExampleDataFile('sphere2500.txt');
model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
[graph,initial] = load3D(datafile, model, true, 2500);
plot3DTrajectory(initial, 'g-', false); % Plot Initial Estimate
%% Read again, now with all constraints, and optimize
graph = load3D(datafile, model, false, 2500);
graph.add(NonlinearEqualityPose3(0, initial.atPose3(0)));
optimizer = LevenbergMarquardtOptimizer(graph, initial);
result = optimizer.optimizeSafely();
plot3DTrajectory(result, 'r-', false); axis equal;

9
doc/Code/SFMExample.m Normal file
View File

@ -0,0 +1,9 @@
%% Add factors for all measurements
noise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
for i = 1:length(Z),
for k = 1:length(Z{i})
j = J{i}{k};
G.add(GenericProjectionFactorCal3_S2(
Z{i}{k}, noise, symbol('x', i), symbol('p', j), K));
end
end

24
doc/Code/VisualISAMExample.cpp Normal file
View File

@ -0,0 +1,24 @@
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
// ... first frame initialization omitted ...
// Loop over the different poses, adding the observations to iSAM
for (size_t i = 1; i < poses.size(); ++i) {
// Add factors for each landmark observation
NonlinearFactorGraph graph;
for (size_t j = 0; j < points.size(); ++j) {
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>
(z[i][j], noise,Symbol('x', i), Symbol('l', j), K)
);
}
// Add an initial guess for the current pose
Values initialEstimate;
initialEstimate.insert(Symbol('x', i), initial_x[i]);
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
}

7
doc/Code/calls.txt Normal file
View File

@ -0,0 +1,7 @@
>> graph.error(initialEstimate)
ans =
20.1086
>> graph.error(result)
ans =
8.2631e-18

23
doc/Code/print.txt Normal file
View File

@ -0,0 +1,23 @@
>> priorNoise
diagonal sigmas [0.3; 0.3; 0.1];
>> graph
size: 6
factor 0: PriorFactor on 1
prior mean: (0, 0, 0)
noise model: diagonal sigmas [0.3; 0.3; 0.1];
factor 1: BetweenFactor(1,2)
measured: (2, 0, 0)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
factor 2: BetweenFactor(2,3)
measured: (2, 0, 1.6)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
factor 3: BetweenFactor(3,4)
measured: (2, 0, 1.6)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
factor 4: BetweenFactor(4,5)
measured: (2, 0, 1.6)
noise model: diagonal sigmas [0.2; 0.2; 0.1];
factor 5: BetweenFactor(5,2)
measured: (2, 0, 1.6)
noise model: diagonal sigmas [0.2; 0.2; 0.1];

7
doc/Code/whos.txt Normal file
View File

@ -0,0 +1,7 @@
>> whos
Name Size Bytes Class
graph 1x1 112 gtsam.NonlinearFactorGraph
priorNoise 1x1 112 gtsam.noiseModel.Diagonal
model 1x1 112 gtsam.noiseModel.Diagonal
initialEstimate 1x1 112 gtsam.Values
optimizer 1x1 112 gtsam.LevenbergMarquardtOptimizer

124
doc/common_macros.tex Normal file
View File

@ -0,0 +1,124 @@
\global\long\def\Vector#1{{\bf #1}}
\global\long\def\Matrix#1{{\bf #1}}
\global\long\def\eq#1{equation (\ref{eq:=0000231})}
\global\long\def\eye#1{\Vector{I_{#1}}}
\global\long\def\leftsparrow#1{\stackrel{#1}{\leftarrow}}
\global\long\def\rightsparrow#1{\stackrel{#1}{\rightarrow}}
\global\long\def\chain{\mathcal{M}}
\global\long\def\define{\stackrel{\Delta}{=}}
\global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}}
\global\long\def\Norm#1{\Vert#1\Vert}
\global\long\def\SqrNorm#1{\Vert#1\Vert^{2}}
\global\long\def\Ltwo#1{\mathcal{L}^{2}\left(#1\right)}
\global\long\def\Normal#1#2#3{\mathcal{N}(#1;#2,#3)}
\global\long\def\LogNormal#1#2#3{ (#1-#2)^{T} #3^{-1} (#1-#2) }
\global\long\def\SqrMah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}^{2}}
\global\long\def\SqrZMah#1#2{\Vert{#1}\Vert_{#2}^{2}}
\global\long\def\Info#1#2#3{\mathcal{N}^{-1}(#1;#2,#3)}
\providecommand{\half}{\frac{1}{2}}
\global\long\def\Mah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}}
\global\long\def\MahDeriv#1#2#3#4{\biggl(\deriv{#2}{#4}\biggr)^{T} #3^{-1} (#1-#2)}
\global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}}
\global\long\def\argmax#1{\mathop{\textrm{argmax \,}}_{#1}}
\global\long\def\deriv#1#2{\frac{\partial#1}{\partial#2}}
\global\long\def\at#1#2{#1\biggr\rvert_{#2}}
\global\long\def\Jac#1#2#3{ \at{\deriv{#1}{#2}} {#3} }
\global\long\def\Rone{\mathbb{R}}
\global\long\def\Pone{\mathbb{P}}
\global\long\def\Rtwo{\mathbb{R}^{2}}
\global\long\def\Ptwo{\mathbb{P}^{2}}
\global\long\def\Stwo{\mathbb{S}^{2}}
\global\long\def\Complex{\mathbb{C}}
\global\long\def\Z{\mathbb{Z}}
\global\long\def\Rplus{\mathbb{R}^{+}}
\global\long\def\SOtwo{SO(2)}
\global\long\def\sotwo{\mathfrak{so(2)}}
\global\long\def\skew#1{[#1]_{+}}
\global\long\def\SEtwo{SE(2)}
\global\long\def\setwo{\mathfrak{se(2)}}
\global\long\def\Skew#1{[#1]_{\times}}
\global\long\def\Rthree{\mathbb{R}^{3}}
\global\long\def\Pthree{\mathbb{P}^{3}}
\global\long\def\SOthree{SO(3)}
\global\long\def\sothree{\mathfrak{so(3)}}
\global\long\def\Rsix{\mathbb{R}^{6}}
\global\long\def\SEthree{SE(3)}
\global\long\def\sethree{\mathfrak{se(3)}}
\global\long\def\Rn{\mathbb{R}^{n}}
\global\long\def\Afftwo{Aff(2)}
\global\long\def\afftwo{\mathfrak{aff(2)}}
\global\long\def\SLthree{SL(3)}
\global\long\def\slthree{\mathfrak{sl(3)}}
\global\long\def\stirling#1#2{\genfrac{\{}{\}}{0pt}{}{#1}{#2}}
\global\long\def\matlabscript#1#2{\begin{itemize}\item[]\lstinputlisting[caption=#2,label=#1]{#1.m}\end{itemize}}
\global\long\def\atan{\mathop{atan2}}

133
doc/gtsam.bib Normal file
View File

@ -0,0 +1,133 @@
@String { ICCV = {Intl. Conf. on Computer Vision (ICCV)} }
@String { IROS = {IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)} }
@String { CVPR = {IEEE Conf. on Computer Vision and Pattern Recognition (CVPR)} }
@String { IJRR = {Intl. J. of Robotics Research} }
@String { RAS = {Robotics and Autonomous Systems} }
@String { TRO = {{IEEE} Trans. Robotics} }
@String { IT = {{IEEE} Trans. Inform. Theory} }
@String { ISRR = {Proc. of the Intl. Symp. of Robotics Research (ISRR)} }
@inproceedings{Davison03iccv,
title = {Real-Time Simultaneous Localisation and Mapping with a Single Camera},
author = {A.J. Davison},
booktitle = ICCV,
year = {2003},
month = {Oct},
pages = {1403-1410}
}
@inproceedings{Dellaert10iros,
title = {Subgraph-preconditioned Conjugate Gradient for Large Scale SLAM},
author = {F. Dellaert and J. Carlson and V. Ila and K. Ni and C.E. Thorpe},
booktitle = IROS,
year = {2010},
}
@inproceedings{Dellaert99b,
title = {Using the Condensation Algorithm for Robust, Vision-based Mobile Robot Localization},
author = {F. Dellaert and D. Fox and W. Burgard and S. Thrun},
booktitle = CVPR,
year = {1999}
}
@article{Dellaert06ijrr,
title = {Square {Root} {SAM}: Simultaneous Localization and Mapping via Square Root Information Smoothing},
author = {F. Dellaert and M. Kaess},
journal = IJRR,
year = {2006},
month = {Dec},
number = {12},
pages = {1181--1203},
volume = {25},
}
@article{DurrantWhyte06ram,
title = {Simultaneous Localisation and Mapping ({SLAM}): Part {I} The Essential Algorithms},
author = {H.F. Durrant-Whyte and T. Bailey},
journal = {Robotics \& Automation Magazine},
year = {2006},
month = {Jun},
}
@inproceedings{Jian11iccv,
title = {Generalized Subgraph Preconditioners for Large-Scale Bundle Adjustment},
author = {Y.-D. Jian and D. Balcan and F. Dellaert},
booktitle = ICCV,
year = {2011},
}
@article{Kaess09ras,
title = {Covariance Recovery from a Square Root Information Matrix for Data Association},
author = {M. Kaess and F. Dellaert},
journal = RAS,
year = {2009},
}
@article{Kaess12ijrr,
title = {{iSAM2}: Incremental Smoothing and Mapping Using the {B}ayes Tree},
author = {M. Kaess and H. Johannsson and R. Roberts and V. Ila and J. Leonard and F. Dellaert},
journal = IJRR,
year = {2012},
month = {Feb},
pages = {217--236},
volume = {31},
issue = {2},
}
@article{Kaess08tro,
title = {{iSAM}: Incremental Smoothing and Mapping},
author = {M. Kaess and A. Ranganathan and F. Dellaert},
journal = TRO,
year = {2008},
month = {Dec},
number = {6},
pages = {1365-1378},
volume = {24},
}
@book{Koller09book,
title = {Probabilistic Graphical Models: Principles and Techniques},
author = {D. Koller and N. Friedman},
publisher = {The MIT Press},
year = {2009}
}
@Article{Kschischang01it,
title = {Factor Graphs and the Sum-Product Algorithm},
Author = {F.R. Kschischang and B.J. Frey and H-A. Loeliger},
Journal = IT,
Year = {2001},
Month = {February},
Number = {2},
Volume = {47}
}
@article{Loeliger04spm,
Title = {An Introduction to Factor Graphs},
Author = {H.-A. Loeliger},
Journal = {IEEE Signal Processing Magazine},
Year = {2004},
Month = {January},
Pages = {28--41}
}
@inproceedings{Nister04cvpr2,
title = {Visual Odometry},
author = {D. Nist\'er and O. Naroditsky and J. Bergen},
booktitle = CVPR,
year = {2004},
month = {Jun},
pages = {652-659},
volume = {1}
}
@InProceedings{Smith87b,
title = {A stochastic map for uncertain spatial relationships},
Author = {R. Smith and M. Self and P. Cheeseman},
Booktitle = ISRR,
Year = {1988},
Pages = {467-474}
}

3767
doc/gtsam.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/gtsam.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/Beijing.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/FactorGraph.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/FactorGraph2.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/FactorGraph3.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/FactorGraph4.pdf Normal file
View File

Binary file not shown.

101
doc/images/Localization.pdf Normal file
View File

@ -0,0 +1,101 @@
%PDF-1.4
%Çì<C387>¢
5 0 obj
<</Length 6 0 R/Filter /FlateDecode>>
stream
xœÍ˜ÏŠ$7 ÆïõuLQlKþw „œgg °<> a&°ä<C2B0>×<EFBFBD>ì²,¹«Yº‡Þ!»,ßÈ®Ÿ¿’,Õ~Ýøݵ¿ãçç·íçOyÿòÏöu 1aÙ“‹JÙߦâK"ð~Ýb‰R¸¢Èª×íÏí÷ýï<C3BD> ¶?û¿›ßÿâ¿mµ$@ç÷€!Cʼ¿(1G¶ú>]QdÕëiŸ×íysW÷?¯}>=óíÊ3Ÿob}>íë^ßâúö½K‚ šBNP‹<50>ÑNÁ9(aøõ…߸並<C2A9>Ÿßö_^8J^þØŽ ñ{%H…Ê‚‡êËþò¶ýð“ƒøãË_Û¯/ÛÓæ)UˆÕRMÅ` ÍpMEÁn…òÄ VÒ唨d„š-ÑT ÑÐ ÑTÞATPŕȸB*`˜TQ&Ñ”I•û™B (9/L^‰R­d ÑT ÑÐ ÑTÞA”`Y]òÆ%t)Z—TQ&Ñ”I•û™Ðä´º”ˆ²‡l]RÅ ÍMåDä äÕ¥`]*ùââQÅ0 Í0MåL%B¢• '…ìøš0Dª(‘hJ¤ÊýDx“D+‘q‰RŽà¬Kª¦¡¦©¼ƒ‰¯Úˆ+M¢èÚ<C3A8>g]RE‰D3-j*÷E^ââ‘qi6ž­_ ¿jU–V7£bH
-Q·2¾–‚çšæ<O1-%œ‘
ÿžÑ ÒP¤5‘LÔ­HüÊ+µË¸xp5HþŠO°g‡B‰b¡4J lÔ½>ahç­×Ÿ‘V h‘†² ͨ‰d¢îõ ù"o ø4 9ì½T¡D±P%P6êæÅAF~iĹÞÖu˜Dü+@k“( ÑŒšD&êæ7硤Py-lN§i`2
Ìn™†²0ͨÉd¢îv‰áÐÓÅ40ÇM¾Š X—D±D%D6ê^—"çwNé4 L¦Dž›<C5BE>eÊÂ4£&“‰º×¥˜</¦<>ÿëGÆŸïüT}‚ÏPú×THàÂYн®}¾µÏ+™ÖÉB±3—V¬tN[ÃOßùÓò`#gÀ<Õ
ÜêD±Ÿäxħ`>?Èy‚@c°Äæ‹ÞJèjïËöDzîÄãF\œàQe® ”Í^”ù·).{©c²—*ºŸ¯r¥˜(O¥r³†[ÏFÓ(Y§Q<íö<"Ríô%ƒFˆ¦Q²N£xÚ‰€d¢ØS¦ëE”h%ë4ªÕžÛ?=ðe>Òµï]_röÈ­&¡=>÷ýöù§Ç×M£sqž×qTÌ\Y¢D9ßvçÏÍÐ^|EðÔ•±“£åø
ðg%ynBÔFšù’øW€¡+ÃD£„öU³¬â~ÍO%<25>º€¥Ï·ä=a殤6<C2A4>1NAÅ(X9«b0«°òÍQÑlŒ•Úÿ|è³±bO<åÃz™êŒ2ÎiV /¸AŒaNüÂZzq¿O%/}ÿ˜kq¤Ô¯´G^TP<çðŽ¶i‰píðÓ¢+Až¯å²¥N¢µƒœ ÇP+µ#Í|FÏø1kÊ£wÇ@7«bºený•Ì"ç8é¼)ÉPË(R‡à|WrO¹#›Â©šÓ|1V‘›NWÉk—<6B>±æ~‹ëÓµpÖ›œA+GÎi•Ã
]t˜¥e,~jm©ç’I—ïåCkë‘=þ‘½ôƒŠ‹2uß´º(3Vµ Æ\3Dm»\ÇQ¹t»l”(Rq”s·A+nî4Kƒ2vc´žæKš½išhî9ÚUɹ'UH±¬ÅM1Å=›•(Ú¬Æ"MrÙv–<76><Y+Eè´3É ¬rœÒ¬Nh™Š[ZOê¨$Ï¥ëG:=mÿF3#òendstream
endobj
6 0 obj
1341
endobj
4 0 obj
<</Type/Page/MediaBox [0 0 612 792]
/Rotate 0/Parent 3 0 R
/Resources<</ProcSet[/PDF /Text]
/ExtGState 9 0 R
/Font 10 0 R
>>
/Contents 5 0 R
>>
endobj
3 0 obj
<< /Type /Pages /Kids [
4 0 R
] /Count 1
>>
endobj
1 0 obj
<</Type /Catalog /Pages 3 0 R
/Metadata 12 0 R
>>
endobj
7 0 obj
<</Type/ExtGState
/OPM 1>>endobj
9 0 obj
<</R7
7 0 R>>
endobj
10 0 obj
<</R8
8 0 R>>
endobj
8 0 obj
<</BaseFont/Helvetica/Type/Font
/Encoding 11 0 R/Subtype/Type1>>
endobj
11 0 obj
<</Type/Encoding/Differences[
45/minus]>>
endobj
12 0 obj
<</Length 1324>>stream
<?xpacket begin='' id='W5M0MpCehiHzreSzNTczkc9d'?>
<?adobe-xap-filters esc="CRLF"?>
<x:xmpmeta xmlns:x='adobe:ns:meta/' x:xmptk='XMP toolkit 2.9.1-13, framework 1.6'>
<rdf:RDF xmlns:rdf='http://www.w3.org/1999/02/22-rdf-syntax-ns#' xmlns:iX='http://ns.adobe.com/iX/1.0/'>
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:pdf='http://ns.adobe.com/pdf/1.3/' pdf:Producer='Artifex Ghostscript 8.54'/>
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:xap='http://ns.adobe.com/xap/1.0/' xap:ModifyDate='2012-06-12' xap:CreateDate='2012-06-12'><xap:CreatorTool>Artifex Ghostscript 8.54 PDF Writer</xap:CreatorTool></rdf:Description>
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:xapMM='http://ns.adobe.com/xap/1.0/mm/' xapMM:DocumentID='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d'/>
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:dc='http://purl.org/dc/elements/1.1/' dc:format='application/pdf'><dc:title><rdf:Alt><rdf:li xml:lang='x-default'>/private/tmp/tpfd5bb1cf_55d8_447e_8f97_d6b24a262922.ps</rdf:li></rdf:Alt></dc:title></rdf:Description>
</rdf:RDF>
</x:xmpmeta>
<?xpacket end='w'?>
endstream
endobj
2 0 obj
<</Producer(Artifex Ghostscript 8.54)
/CreationDate(D:20120612011010)
/ModDate(D:20120612011010)
/Creator(MATLAB, The MathWorks, Inc. Version 7.13.0.564 \(R2011b\). Operating System: Darwin 11.4.0 Darwin Kernel Version 11.4.0: Mon Apr 9 19:32:15 PDT 2012; root:xnu-1699.26.8~1/RELEASE_X86_64 x86_64.)
/Title(/private/tmp/tpfd5bb1cf_55d8_447e_8f97_d6b24a262922.ps)>>endobj
xref
0 13
0000000000 65535 f
0000001664 00000 n
0000003341 00000 n
0000001605 00000 n
0000001446 00000 n
0000000015 00000 n
0000001426 00000 n
0000001729 00000 n
0000001829 00000 n
0000001770 00000 n
0000001799 00000 n
0000001909 00000 n
0000001967 00000 n
trailer
<< /Size 13 /Root 1 0 R /Info 2 0 R
/ID [<0E15732D337F57A9CB2C0954A12E8EF8><0E15732D337F57A9CB2C0954A12E8EF8>]
>>
startxref
3722
%%EOF

BIN
doc/images/Odometry.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/Victoria.pdf Normal file
View File

Binary file not shown.

559
doc/images/cube.pdf Normal file
View File

File diff suppressed because one or more lines are too long

BIN
doc/images/example1.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/example2.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/hmm-FG.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/hmm.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/littleRobot.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/sphere2500-result.pdf Normal file
View File

Binary file not shown.

BIN
doc/images/w100-result.pdf Normal file
View File

Binary file not shown.

2
doc/math.lyx
View File

@ -5189,7 +5189,7 @@ R_{2}^{T}\left[t_{2}-t_{1}\right]_{\times}R_{1} & -R_{2}^{T}R_{1}
and in its second argument,
\begin_inset Formula
\begin{eqnarray*}
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6}
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{2}} & = & I_{6}
\end{eqnarray*}
\end_inset

18
docker/ubuntu-boost-tbb-eigen3/Dockerfile Normal file
View File

@ -0,0 +1,18 @@
# Get the base Ubuntu image from Docker Hub
FROM ubuntu:bionic
# Update apps on the base image
RUN apt-get -y update && apt-get install -y
# Install C++
RUN apt-get -y install build-essential
# Install boost and cmake
RUN apt-get -y install libboost-all-dev cmake
# Install TBB
RUN apt-get -y install libtbb-dev
# Install latest Eigen
RUN apt-get install -y libeigen3-dev

120
examples/ISAM2Example_SmartFactor.cpp Normal file
View File

@ -0,0 +1,120 @@
/**
* @file ISAM2Example_SmartFactor.cpp
* @brief test of iSAM with smart factors, led to bitbucket issue #367
* @author Alexander (pumaking on BitBucket)
*/
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <iostream>
#include <vector>
using namespace std;
using namespace gtsam;
using symbol_shorthand::P;
using symbol_shorthand::X;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
int main(int argc, char* argv[]) {
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
Vector6 sigmas;
sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1);
auto noise = noiseModel::Diagonal::Sigmas(sigmas);
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
parameters.cacheLinearizedFactors = false;
parameters.enableDetailedResults = true;
parameters.print();
ISAM2 isam(parameters);
// Create a factor graph
NonlinearFactorGraph graph;
Values initialEstimate;
Point3 point(0.0, 0.0, 1.0);
// Intentionally initialize the variables off from the ground truth
Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20));
Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0));
Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0));
Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0));
Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0));
Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0));
vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
// Add first pose
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
initialEstimate.insert(X(0), poses[0].compose(delta));
// Create smart factor with measurement from first pose only
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
graph.push_back(smartFactor);
// loop over remaining poses
for (size_t i = 1; i < 5; i++) {
cout << "****************************************************" << endl;
cout << "i = " << i << endl;
// Add prior on new pose
graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
initialEstimate.insert(X(i), poses[i].compose(delta));
// "Simulate" measurement from this pose
PinholePose<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(point);
cout << "Measurement " << i << "" << measurement << endl;
// Add measurement to smart factor
smartFactor->add(measurement, X(i));
// Update iSAM2
ISAM2Result result = isam.update(graph, initialEstimate);
result.print();
cout << "Detailed results:" << endl;
for (auto keyedStatus : result.detail->variableStatus) {
const auto& status = keyedStatus.second;
PrintKey(keyedStatus.first);
cout << " {" << endl;
cout << "reeliminated: " << status.isReeliminated << endl;
cout << "relinearized above thresh: " << status.isAboveRelinThreshold
<< endl;
cout << "relinearized involved: " << status.isRelinearizeInvolved << endl;
cout << "relinearized: " << status.isRelinearized << endl;
cout << "observed: " << status.isObserved << endl;
cout << "new: " << status.isNew << endl;
cout << "in the root clique: " << status.inRootClique << endl;
cout << "}" << endl;
}
Values currentEstimate = isam.calculateEstimate();
currentEstimate.print("Current estimate: ");
boost::optional<Point3> pointEstimate = smartFactor->point(currentEstimate);
if (pointEstimate) {
cout << *pointEstimate << endl;
} else {
cout << "Point degenerate." << endl;
}
// Reset graph and initial estimate for next iteration
graph.resize(0);
initialEstimate.clear();
}
return 0;
}

136
examples/ImuFactorExample2.cpp Normal file
View File

@ -0,0 +1,136 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <vector>
using namespace std;
using namespace gtsam;
// Shorthand for velocity and pose variables
using symbol_shorthand::V;
using symbol_shorthand::X;
const double kGravity = 9.81;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
auto params = PreintegrationParams::MakeSharedU(kGravity);
params->setAccelerometerCovariance(I_3x3 * 0.1);
params->setGyroscopeCovariance(I_3x3 * 0.1);
params->setIntegrationCovariance(I_3x3 * 0.1);
params->setUse2ndOrderCoriolis(false);
params->setOmegaCoriolis(Vector3(0, 0, 0));
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
// Start with a camera on x-axis looking at origin
double radius = 30;
const Point3 up(0, 0, 1), target(0, 0, 0);
const Point3 position(radius, 0, 0);
const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
const auto pose_0 = camera.pose();
// Now, create a constant-twist scenario that makes the camera orbit the
// origin
double angular_velocity = M_PI, // rad/sec
delta_t = 1.0 / 18; // makes for 10 degrees per step
Vector3 angular_velocity_vector(0, -angular_velocity, 0);
Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
auto scenario = ConstantTwistScenario(angular_velocity_vector,
linear_velocity_vector, pose_0);
// Create a factor graph
NonlinearFactorGraph newgraph;
// Create (incremental) ISAM2 solver
ISAM2 isam;
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate, totalEstimate, result;
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
// Add imu priors
Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
biasnoise);
newgraph.push_back(biasprior);
initialEstimate.insert(biasKey, imuBias::ConstantBias());
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
Vector n_velocity(3);
n_velocity << 0, angular_velocity * radius, 0;
PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
newgraph.push_back(velprior);
initialEstimate.insert(V(0), n_velocity);
// IMU preintegrator
PreintegratedImuMeasurements accum(params);
// Simulate poses and imu measurements, adding them to the factor graph
for (size_t i = 0; i < 36; ++i) {
double t = i * delta_t;
if (i == 0) { // First time add two poses
auto pose_1 = scenario.pose(delta_t);
initialEstimate.insert(X(0), pose_0.compose(delta));
initialEstimate.insert(X(1), pose_1.compose(delta));
} else if (i >= 2) { // Add more poses as necessary
auto pose_i = scenario.pose(t);
initialEstimate.insert(X(i), pose_i.compose(delta));
}
if (i > 0) {
// Add Bias variables periodically
if (i % 5 == 0) {
biasKey++;
Symbol b1 = biasKey - 1;
Symbol b2 = biasKey;
Vector6 covvec;
covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
auto cov = noiseModel::Diagonal::Variances(covvec);
Vector6 zerovec;
zerovec << 0, 0, 0, 0, 0, 0;
auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
b1, b2, imuBias::ConstantBias(), cov);
newgraph.add(f);
initialEstimate.insert(biasKey, imuBias::ConstantBias());
}
// Predict acceleration and gyro measurements in (actual) body frame
auto measuredAcc = scenario.acceleration_b(t) -
scenario.rotation(t).transpose() * params->n_gravity;
auto measuredOmega = scenario.omega_b(t);
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
// Add Imu Factor
ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
newgraph.add(imufac);
// insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity);
accum.resetIntegration();
}
// Incremental solution
isam.update(newgraph, initialEstimate);
result = isam.calculateEstimate();
newgraph = NonlinearFactorGraph();
initialEstimate.clear();
}
GTSAM_PRINT(result);
return 0;
}
/* ************************************************************************* */

2
examples/SolverComparer.cpp
View File

@ -206,7 +206,7 @@ int main(int argc, char *argv[]) {
}
#ifdef GTSAM_USE_TBB
std::auto_ptr<tbb::task_scheduler_init> init;
std::unique_ptr<tbb::task_scheduler_init> init;
if(nThreads > 0) {
cout << "Using " << nThreads << " threads" << endl;
init.reset(new tbb::task_scheduler_init(nThreads));

Some files were not shown because too many files have changed in this diff Show More