diff --git a/CMakeLists.txt b/CMakeLists.txt index 8cf63f4f1..d82e31228 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -96,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") @@ -360,6 +364,11 @@ add_subdirectory(examples) # Build timing add_subdirectory(timing) +# Build gtsam_unstable +if (GTSAM_BUILD_UNSTABLE) + add_subdirectory(gtsam_unstable) +endif(GTSAM_BUILD_UNSTABLE) + # Matlab toolbox if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) @@ -381,13 +390,18 @@ endif() # Cython wrap if (GTSAM_INSTALL_CYTHON_TOOLBOX) - add_subdirectory(cython) + 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() -# Build gtsam_unstable -if (GTSAM_BUILD_UNSTABLE) - add_subdirectory(gtsam_unstable) -endif(GTSAM_BUILD_UNSTABLE) # Install config and export files GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index b506a8f36..f2ca9933e 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -21,6 +21,7 @@ install(FILES GtsamCythonWrap.cmake GtsamTesting.cmake FindCython.cmake + FindNumPy.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake index e2995bff3..23afb00e6 100644 --- a/cmake/FindCython.cmake +++ b/cmake/FindCython.cmake @@ -4,8 +4,9 @@ # # This code sets the following variables: # -# CYTHON_EXECUTABLE # CYTHON_FOUND +# CYTHON_PATH +# CYTHON_EXECUTABLE # CYTHON_VERSION # # See also UseCython.cmake @@ -38,6 +39,15 @@ if ( PYTHONINTERP_FOUND ) ) 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" @@ -59,6 +69,7 @@ find_package_handle_standard_args( Cython CYTHON_FOUND REQUIRED_VARS CYTHON_PATH + CYTHON_EXECUTABLE VERSION_VAR CYTHON_VERSION ) diff --git a/cmake/FindEigency.cmake b/cmake/FindEigency.cmake deleted file mode 100644 index 3e48dab64..000000000 --- a/cmake/FindEigency.cmake +++ /dev/null @@ -1,27 +0,0 @@ -# Find Eigency -# -# This code sets the following variables: -# -# EIGENCY_FOUND -# EIGENCY_INCLUDE_DIRS -# - -# Find python -find_package( PythonInterp ) -if ( PYTHONINTERP_FOUND ) - execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import eigency; includes=eigency.get_includes(include_eigen=False); print includes[0], ';', includes[1]" - RESULT_VARIABLE RESULT - OUTPUT_VARIABLE EIGENCY_INCLUDE_DIRS - OUTPUT_STRIP_TRAILING_WHITESPACE - ) -endif () - -include( FindPackageHandleStandardArgs ) -find_package_handle_standard_args(Eigency - FOUND_VAR - EIGENCY_FOUND - REQUIRED_VARS - EIGENCY_INCLUDE_DIRS -) - diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 92f4981a7..d4bff5537 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -5,12 +5,6 @@ unset(PYTHON_EXECUTABLE CACHE) unset(CYTHON_EXECUTABLE CACHE) find_package(Cython 0.25.2 REQUIRED) -# 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() - # User-friendly Cython wrapping and installing function. # Builds a Cython module from the provided interface_header. # For example, for the interface header gtsam.h, @@ -23,18 +17,94 @@ endif() # 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 dependencies) +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}" "${dependencies}") + wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}") install_cython_wrapped_library("${interface_header}" "${generated_files_path}" "${install_path}") endfunction() +function(set_up_required_cython_packages) + # Set up building of cython module + find_package(PythonLibs 2.7 REQUIRED) + include_directories(${PYTHON_INCLUDE_DIRS}) + find_package(NumPy REQUIRED) + include_directories(${NUMPY_INCLUDE_DIRS}) +endfunction() + +# Convert pyx to cpp by executing cython +# This is the first step to compile cython from the command line +# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html +# +# Arguments: +# - target: The specified target for this step +# - pyx_file: The input pyx_file in full *absolute* path +# - generated_cpp: The output cpp file in full absolute path +# - include_dirs: Directories to include when executing cython +function(pyx_to_cpp target pyx_file generated_cpp include_dirs) + foreach(dir ${include_dirs}) + set(includes_for_cython ${includes_for_cython} -I ${dir}) + endforeach() + + add_custom_command( + OUTPUT ${generated_cpp} + COMMAND + ${CYTHON_EXECUTABLE} -X boundscheck=False -a -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp} + VERBATIM) + add_custom_target(${target} ALL DEPENDS ${generated_cpp}) +endfunction() + +# Build the cpp file generated by converting pyx using cython +# This is the second step to compile cython from the command line +# as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html +# +# Arguments: +# - target: The specified target for this step +# - cpp_file: The input cpp_file in full *absolute* path +# - output_lib_we: The output lib filename only (without extension) +# - output_dir: The output directory +function(build_cythonized_cpp target cpp_file output_lib_we output_dir) + add_library(${target} MODULE ${cpp_file}) + if(APPLE) + set(link_flags "-undefined dynamic_lookup") + endif() + set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "${link_flags}" + OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir}) +endfunction() + +# Cythonize a pyx from the command line as described at +# http://cython.readthedocs.io/en/latest/src/reference/compilation.html +# Arguments: +# - target: The specified target +# - pyx_file: The input pyx_file in full *absolute* path +# - output_lib_we: The output lib filename only (without extension) +# - output_dir: The output directory +# - include_dirs: Directories to include when executing cython +# - libs: Libraries to link with +# - dependencies: Other target dependencies +function(cythonize target pyx_file output_lib_we output_dir include_dirs libs 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}") + 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 dependencies) +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* @@ -42,39 +112,26 @@ function(wrap_library_cython interface_header generated_files_path extra_imports get_filename_component(module_path "${interface_header}" PATH) get_filename_component(module_name "${interface_header}" NAME_WE) - set(generated_cpp_file "${generated_files_path}/${module_name}.cpp") - - message(STATUS "Building wrap module ${module_name}") - - # Set up generation of module source file + # 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_cpp_file} + OUTPUT ${generated_pyx} DEPENDS ${interface_header} wrap - COMMAND - wrap --cython - ${module_path} - ${module_name} - ${generated_files_path} - "${extra_imports}" - && cython --cplus -I. "${generated_files_path}/${module_name}.pyx" + COMMAND + wrap --cython ${module_path} ${module_name} ${generated_files_path} "${extra_imports}" VERBATIM WORKING_DIRECTORY ${generated_files_path}/../) - add_custom_target(${module_name}_cython_wrapper ALL DEPENDS ${generated_cpp_file} ${interface_header} ${dependencies}) + 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() - # Set up building of cython module - find_package(PythonLibs 2.7 REQUIRED) - include_directories(${PYTHON_INCLUDE_DIRS}) - find_package(Eigency REQUIRED) - include_directories(${EIGENCY_INCLUDE_DIRS}) - find_package(NumPy REQUIRED) - include_directories(${NUMPY_INCLUDE_DIRS}) - - add_library(${module_name}_cython MODULE ${generated_cpp_file}) - set_target_properties(${module_name}_cython PROPERTIES LINK_FLAGS "-undefined dynamic_lookup" - OUTPUT_NAME ${module_name} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${generated_files_path}) - target_link_libraries(${module_name}_cython ${module_name}) - add_dependencies(${module_name}_cython ${module_name}_cython_wrapper) + message(STATUS "Cythonize and build ${module_name}.pyx") + get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) + cythonize(${module_name}_cython ${generated_pyx} ${module_name} + ${generated_files_path} "${include_dirs}" "${libs}" cython_wrap_${module_name}_pyx) # distclean add_custom_target(wrap_${module_name}_cython_distclean diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index ea00e18d5..c264b8f37 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -3,24 +3,34 @@ 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 # dependencies which need to be built before the wrapper + 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 # dependencies which need to be built before the wrapper + gtsam_unstable # library to link with + "gtsam_unstable;gtsam_unstable_header;gtsam_cython" # dependencies to be built before wrapping ) - add_dependencies(gtsam_unstable_cython_wrapper gtsam_cython_wrapper) + # 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 with gtsam_unstable disabled + # 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") diff --git a/cython/README.md b/cython/README.md index bc616ec7f..2b1fa28d1 100644 --- a/cython/README.md +++ b/cython/README.md @@ -8,17 +8,11 @@ INSTALL pip install -r /cython/requirements.txt ``` -- It also needs Eigency, a package that interfaces between C++'s Eigen and Python's numpy. -You can try to install via pip: "pip install eigency". If that fails, please install it from source as follows: +- 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. -```bash -git clone https://github.com/wouterboomsma/eigency.git -cd eigency -python setup.py -v build -python setup.py install -``` - -- 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 +- Build and install gtsam using cmake with GTSAM_INSTALL_CYTHON_TOOLBOX enabled. +The wrapped module will be installed to GTSAM_CYTHON_INSTALL_PATH, which is by default: /cython - Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH: @@ -26,7 +20,6 @@ by default: /cython export PYTHONPATH = $PYTHONPATH: ``` - UNIT TESTS ========== The Cython toolbox also has a small set of unit tests located in the @@ -68,48 +61,26 @@ Examples: 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 -- Create your setup.py.in as follows: -```python -from distutils.core import setup -from distutils.extension import Extension -from Cython.Build import cythonize -import eigency - -include_dirs = ["${CMAKE_SOURCE_DIR}/cpp", "${CMAKE_BINARY_DIR}"] -include_dirs += "${GTSAM_INCLUDE_DIR}".split(";") -include_dirs += eigency.get_includes(include_eigen=False) - -setup( - ext_modules=cythonize(Extension( - "your_module_name", - sources=["your_module_name.pyx"], - include_dirs= include_dirs, - libraries=['gtsam'], - library_dirs=["${GTSAM_DIR}/../../"], - language="c++", - extra_compile_args=["-std=c++11"])), -) - -``` - - 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 - "path_to_your_setup.py.in" "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 diff --git a/cython/gtsam/tests/gtsam_test.h b/cython/gtsam/tests/gtsam_test.h index 4f161660b..659a7cd0c 100644 --- a/cython/gtsam/tests/gtsam_test.h +++ b/cython/gtsam/tests/gtsam_test.h @@ -762,4 +762,11 @@ namespace utilities { gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); } //\namespace utilities + +#include +class RedirectCout { + RedirectCout(); + string str(); +}; + } //\namespace gtsam diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index b7097a24e..3225d2ff9 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -1,6 +1,5 @@ import unittest import gtsam -from math import * import numpy as np @@ -11,5 +10,13 @@ class TestCal3Unified(unittest.TestCase): self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) + def test_retract(self): + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') + actual = K.retract(d) + self.assertTrue(actual.equals(expected, 1e-9)) + np.testing.assert_allclose(d, K.localCoordinates(actual)) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py new file mode 100644 index 000000000..8fa50b90c --- /dev/null +++ b/cython/gtsam/tests/test_Pose3.py @@ -0,0 +1,44 @@ +import math +import unittest + +from gtsam import Point3, Rot3, Pose3 + + +class TestPose3(unittest.TestCase): + + def test__between(self): + T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)) + T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) + expected = T2.inverse().compose(T3) + actual = T2.between(T3) + self.assertTrue(actual.equals(expected, 1e-6)) + + def test_transform_to(self): + transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0)) + actual = transform.transform_to(Point3(3,2,10)) + expected = Point3 (2,1,10) + self.assertTrue(actual.equals(expected, 1e-6)) + + def test_range(self): + l1 = Point3(1, 0, 0) + l2 = Point3(1, 1, 0) + x1 = Pose3() + + xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)) + xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + + # establish range is indeed zero + self.assertEqual(1,x1.range(point=l1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0),x1.range(point=l2)) + + # establish range is indeed zero + self.assertEqual(1,x1.range(pose=xl1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py new file mode 100644 index 000000000..7924a9b1c --- /dev/null +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -0,0 +1,32 @@ +import math +import numpy as np +import unittest + +from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera + +K = Cal3_S2(625, 625, 0, 0, 0) + +class TestSimpleCamera(unittest.TestCase): + + def test_constructor(self): + pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) + camera = SimpleCamera(pose1, K) + self.assertTrue(camera.calibration().equals(K, 1e-9)) + self.assertTrue(camera.pose().equals(pose1, 1e-9)) + + def test_level2(self): + # Create a level camera, looking in Y-direction + pose2 = Pose2(0.4,0.3,math.pi/2.0) + camera = SimpleCamera.Level(K, pose2, 0.1) + + # expected + x = Point3(1,0,0) + y = Point3(0,0,-1) + z = Point3(0,1,0) + wRc = Rot3(x,y,z) + expected = Pose3(wRc,Point3(0.4,0.3,0.1)) + self.assertTrue(camera.pose().equals(expected, 1e-9)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index cca38d8cd..08e133840 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -1,25 +1,28 @@ import unittest -import gtsam 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 = gtsam.Values() - E = gtsam.EssentialMatrix(gtsam.Rot3(), gtsam.Unit3()) + values = Values() + E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 - values.insert(0, gtsam.Point2()) - values.insert(1, gtsam.Point3()) - values.insert(2, gtsam.Rot2()) - values.insert(3, gtsam.Pose2()) - values.insert(4, gtsam.Rot3()) - values.insert(5, gtsam.Pose3()) - values.insert(6, gtsam.Cal3_S2()) - values.insert(7, gtsam.Cal3DS2()) - values.insert(8, gtsam.Cal3Bundler()) + 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, gtsam.imuBias_ConstantBias()) + values.insert(10, imuBias_ConstantBias()) # Special cases for Vectors and Matrices # Note that gtsam's Eigen Vectors and Matrices requires double-precision @@ -29,7 +32,7 @@ class TestValues(unittest.TestCase): # will have 'int' type. # # The wrapper will automatically fix the type and storage order for you, - # but for performace reasons, it's recommended to specify the correct + # 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) @@ -41,18 +44,18 @@ class TestValues(unittest.TestCase): mat2 = np.array([[1,2,],[3,5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(gtsam.Point2(), tol)) - self.assertTrue(values.atPoint3(1).equals(gtsam.Point3(), tol)) - self.assertTrue(values.atRot2(2).equals(gtsam.Rot2(), tol)) - self.assertTrue(values.atPose2(3).equals(gtsam.Pose2(), tol)) - self.assertTrue(values.atRot3(4).equals(gtsam.Rot3(), tol)) - self.assertTrue(values.atPose3(5).equals(gtsam.Pose3(), tol)) - self.assertTrue(values.atCal3_S2(6).equals(gtsam.Cal3_S2(), tol)) - self.assertTrue(values.atCal3DS2(7).equals(gtsam.Cal3DS2(), tol)) - self.assertTrue(values.atCal3Bundler(8).equals(gtsam.Cal3Bundler(), tol)) + 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(gtsam.imuBias_ConstantBias(), tol)) + 10).equals(imuBias_ConstantBias(), tol)) # special cases for Vector and Matrix: actualVector = values.atVector(11) diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt new file mode 100644 index 000000000..5fa9e7cc0 --- /dev/null +++ b/cython/gtsam_eigency/CMakeLists.txt @@ -0,0 +1,38 @@ +include(GtsamCythonWrap) + +# Copy eigency's sources to the build folder +# so that the cython-generated header "conversions_api.h" can be found when cythonizing eigency's core +# and eigency's cython pxd headers can be found when cythonizing gtsam +file(COPY "." DESTINATION ".") +set(OUTPUT_DIR "${PROJECT_BINARY_DIR}/cython/gtsam_eigency") +set(EIGENCY_INCLUDE_DIR ${OUTPUT_DIR}) + +# This is to make the build/cython/gtsam_eigency folder a python package +configure_file(__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_eigency/__init__.py) + +# include eigency headers +include_directories(${EIGENCY_INCLUDE_DIR}) + +# Cythonize and build eigency +message(STATUS "Cythonize and build eigency") +# Important trick: use "../gtsam_eigency/conversions.pyx" to let cython know that the conversions module is +# a part of the gtsam_eigency package and generate the function call import_gtsam_igency__conversions() +# in conversions_api.h correctly!!! +cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conversions" + "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "") +cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core" + ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "") +add_dependencies(cythonize_eigency_core cythonize_eigency_conversions) +add_custom_target(cythonize_eigency) +add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core) + +# install +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DESTINATION ${GTSAM_CYTHON_INSTALL_PATH} + PATTERN "CMakeLists.txt" EXCLUDE + PATTERN "__init__.py.in" EXCLUDE) +install(TARGETS cythonize_eigency_core cythonize_eigency_conversions + DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency") +install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) +configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py) +install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) diff --git a/cython/gtsam_eigency/LICENSE.txt b/cython/gtsam_eigency/LICENSE.txt new file mode 100644 index 000000000..71743c864 --- /dev/null +++ b/cython/gtsam_eigency/LICENSE.txt @@ -0,0 +1,20 @@ +Copyright (c) 2016 Wouter Boomsma + +Permission is hereby granted, free of charge, to any person obtaining +a copy of this software and associated documentation files (the +"Software"), to deal in the Software without restriction, including +without limitation the rights to use, copy, modify, merge, publish, +distribute, sublicense, and/or sell copies of the Software, and to +permit persons to whom the Software is furnished to do so, subject to +the following conditions: + +The above copyright notice and this permission notice shall be +included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE +LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION +OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION +WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/cython/gtsam_eigency/__init__.py.in b/cython/gtsam_eigency/__init__.py.in new file mode 100644 index 000000000..dd278d128 --- /dev/null +++ b/cython/gtsam_eigency/__init__.py.in @@ -0,0 +1,13 @@ +import os +import numpy as np + +__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_PREFIX}" + +def get_includes(include_eigen=True): + root = os.path.dirname(__file__) + parent = os.path.join(root, "..") + path = [root, parent, np.get_include()] + if include_eigen: + path.append(os.path.join(root, __eigen_dir__)) + return path + diff --git a/cython/gtsam_eigency/conversions.pxd b/cython/gtsam_eigency/conversions.pxd new file mode 100644 index 000000000..f4445e585 --- /dev/null +++ b/cython/gtsam_eigency/conversions.pxd @@ -0,0 +1,62 @@ +cimport numpy as np + +cdef api np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long outer_stride, long inner_stride) + +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) +cdef api np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long outer_stride, long inner_stride) + diff --git a/cython/gtsam_eigency/conversions.pyx b/cython/gtsam_eigency/conversions.pyx new file mode 100644 index 000000000..55c9ae0cd --- /dev/null +++ b/cython/gtsam_eigency/conversions.pyx @@ -0,0 +1,327 @@ +cimport cython +import numpy as np +from numpy.lib.stride_tricks import as_strided + +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_double_C(double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[:,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_double_F(double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[::1,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_copy_double_C(const double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[:,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[double, ndim=2] ndarray_copy_double_F(const double *data, long rows, long cols, long row_stride, long col_stride): + cdef double[::1,:] mem_view = data + dtype = 'double' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_float_C(float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[:,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_float_F(float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[::1,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_copy_float_C(const float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[:,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[float, ndim=2] ndarray_copy_float_F(const float *data, long rows, long cols, long row_stride, long col_stride): + cdef float[::1,:] mem_view = data + dtype = 'float' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_long_C(long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[:,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_long_F(long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[::1,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_copy_long_C(const long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[:,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[long, ndim=2] ndarray_copy_long_F(const long *data, long rows, long cols, long row_stride, long col_stride): + cdef long[::1,:] mem_view = data + dtype = 'int_' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_C(unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_ulong_F(unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_C(const unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned long, ndim=2] ndarray_copy_ulong_F(const unsigned long *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned long[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_int_C(int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[:,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_int_F(int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[::1,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_copy_int_C(const int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[:,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[int, ndim=2] ndarray_copy_int_F(const int *data, long rows, long cols, long row_stride, long col_stride): + cdef int[::1,:] mem_view = data + dtype = 'int' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_C(unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_uint_F(unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_C(const unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[:,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned int, ndim=2] ndarray_copy_uint_F(const unsigned int *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned int[::1,:] mem_view = data + dtype = 'uint' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_short_C(short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[:,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_short_F(short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[::1,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_copy_short_C(const short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[:,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[short, ndim=2] ndarray_copy_short_F(const short *data, long rows, long cols, long row_stride, long col_stride): + cdef short[::1,:] mem_view = data + dtype = 'short' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_C(unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[:,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_ushort_F(unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[::1,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_C(const unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[:,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned short, ndim=2] ndarray_copy_ushort_F(const unsigned short *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned short[::1,:] mem_view = data + dtype = 'ushort' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_schar_C(signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[:,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_schar_F(signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[::1,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_C(const signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[:,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[signed char, ndim=2] ndarray_copy_schar_F(const signed char *data, long rows, long cols, long row_stride, long col_stride): + cdef signed char[::1,:] mem_view = data + dtype = 'int8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_C(unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[:,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_uchar_F(unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[::1,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_C(const unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[:,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[unsigned char, ndim=2] ndarray_copy_uchar_F(const unsigned char *data, long rows, long cols, long row_stride, long col_stride): + cdef unsigned char[::1,:] mem_view = data + dtype = 'uint8' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_C(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[:,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_complex_double_F(np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[::1,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_C(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[:,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[np.complex128_t, ndim=2] ndarray_copy_complex_double_F(const np.complex128_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex128_t[::1,:] mem_view = data + dtype = 'complex128' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + + +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_C(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[:,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize]) +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_complex_float_F(np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[::1,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize]) + +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_C(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[:,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="C"), strides=[row_stride*itemsize, col_stride*itemsize])) +@cython.boundscheck(False) +cdef np.ndarray[np.complex64_t, ndim=2] ndarray_copy_complex_float_F(const np.complex64_t *data, long rows, long cols, long row_stride, long col_stride): + cdef np.complex64_t[::1,:] mem_view = data + dtype = 'complex64' + cdef int itemsize = np.dtype(dtype).itemsize + return np.copy(as_strided(np.asarray(mem_view, dtype=dtype, order="F"), strides=[row_stride*itemsize, col_stride*itemsize])) + diff --git a/cython/gtsam_eigency/core.pxd b/cython/gtsam_eigency/core.pxd new file mode 100644 index 000000000..9a84c3c16 --- /dev/null +++ b/cython/gtsam_eigency/core.pxd @@ -0,0 +1,917 @@ +cimport cython +cimport numpy as np + +ctypedef signed char schar; +ctypedef unsigned char uchar; + +ctypedef fused dtype: + uchar + schar + short + int + long + float + double + +ctypedef fused DenseType: + Matrix + Array + +ctypedef fused Rows: + _1 + _2 + _3 + _4 + _5 + _6 + _7 + _8 + _9 + _10 + _11 + _12 + _13 + _14 + _15 + _16 + _17 + _18 + _19 + _20 + _21 + _22 + _23 + _24 + _25 + _26 + _27 + _28 + _29 + _30 + _31 + _32 + Dynamic + +ctypedef Rows Cols +ctypedef Rows StrideOuter +ctypedef Rows StrideInner + +ctypedef fused DenseTypeShort: + Vector1i + Vector2i + Vector3i + Vector4i + VectorXi + RowVector1i + RowVector2i + RowVector3i + RowVector4i + RowVectorXi + Matrix1i + Matrix2i + Matrix3i + Matrix4i + MatrixXi + Vector1f + Vector2f + Vector3f + Vector4f + VectorXf + RowVector1f + RowVector2f + RowVector3f + RowVector4f + RowVectorXf + Matrix1f + Matrix2f + Matrix3f + Matrix4f + MatrixXf + Vector1d + Vector2d + Vector3d + Vector4d + VectorXd + RowVector1d + RowVector2d + RowVector3d + RowVector4d + RowVectorXd + Matrix1d + Matrix2d + Matrix3d + Matrix4d + MatrixXd + Vector1cf + Vector2cf + Vector3cf + Vector4cf + VectorXcf + RowVector1cf + RowVector2cf + RowVector3cf + RowVector4cf + RowVectorXcf + Matrix1cf + Matrix2cf + Matrix3cf + Matrix4cf + MatrixXcf + Vector1cd + Vector2cd + Vector3cd + Vector4cd + VectorXcd + RowVector1cd + RowVector2cd + RowVector3cd + RowVector4cd + RowVectorXcd + Matrix1cd + Matrix2cd + Matrix3cd + Matrix4cd + MatrixXcd + Array22i + Array23i + Array24i + Array2Xi + Array32i + Array33i + Array34i + Array3Xi + Array42i + Array43i + Array44i + Array4Xi + ArrayX2i + ArrayX3i + ArrayX4i + ArrayXXi + Array2i + Array3i + Array4i + ArrayXi + Array22f + Array23f + Array24f + Array2Xf + Array32f + Array33f + Array34f + Array3Xf + Array42f + Array43f + Array44f + Array4Xf + ArrayX2f + ArrayX3f + ArrayX4f + ArrayXXf + Array2f + Array3f + Array4f + ArrayXf + Array22d + Array23d + Array24d + Array2Xd + Array32d + Array33d + Array34d + Array3Xd + Array42d + Array43d + Array44d + Array4Xd + ArrayX2d + ArrayX3d + ArrayX4d + ArrayXXd + Array2d + Array3d + Array4d + ArrayXd + Array22cf + Array23cf + Array24cf + Array2Xcf + Array32cf + Array33cf + Array34cf + Array3Xcf + Array42cf + Array43cf + Array44cf + Array4Xcf + ArrayX2cf + ArrayX3cf + ArrayX4cf + ArrayXXcf + Array2cf + Array3cf + Array4cf + ArrayXcf + Array22cd + Array23cd + Array24cd + Array2Xcd + Array32cd + Array33cd + Array34cd + Array3Xcd + Array42cd + Array43cd + Array44cd + Array4Xcd + ArrayX2cd + ArrayX3cd + ArrayX4cd + ArrayXXcd + Array2cd + Array3cd + Array4cd + ArrayXcd + +ctypedef fused StorageOrder: + RowMajor + ColMajor + +ctypedef fused MapOptions: + Aligned + Unaligned + +cdef extern from "eigency_cpp.h" namespace "eigency": + + cdef cppclass _1 "1": + pass + + cdef cppclass _2 "2": + pass + + cdef cppclass _3 "3": + pass + + cdef cppclass _4 "4": + pass + + cdef cppclass _5 "5": + pass + + cdef cppclass _6 "6": + pass + + cdef cppclass _7 "7": + pass + + cdef cppclass _8 "8": + pass + + cdef cppclass _9 "9": + pass + + cdef cppclass _10 "10": + pass + + cdef cppclass _11 "11": + pass + + cdef cppclass _12 "12": + pass + + cdef cppclass _13 "13": + pass + + cdef cppclass _14 "14": + pass + + cdef cppclass _15 "15": + pass + + cdef cppclass _16 "16": + pass + + cdef cppclass _17 "17": + pass + + cdef cppclass _18 "18": + pass + + cdef cppclass _19 "19": + pass + + cdef cppclass _20 "20": + pass + + cdef cppclass _21 "21": + pass + + cdef cppclass _22 "22": + pass + + cdef cppclass _23 "23": + pass + + cdef cppclass _24 "24": + pass + + cdef cppclass _25 "25": + pass + + cdef cppclass _26 "26": + pass + + cdef cppclass _27 "27": + pass + + cdef cppclass _28 "28": + pass + + cdef cppclass _29 "29": + pass + + cdef cppclass _30 "30": + pass + + cdef cppclass _31 "31": + pass + + cdef cppclass _32 "32": + pass + + cdef cppclass PlainObjectBase: + pass + + cdef cppclass Map[DenseTypeShort](PlainObjectBase): + Map() except + + Map(np.ndarray array) except + + + cdef cppclass FlattenedMap[DenseType, dtype, Rows, Cols]: + FlattenedMap() except + + FlattenedMap(np.ndarray array) except + + + cdef cppclass FlattenedMapWithOrder "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder]: + FlattenedMapWithOrder() except + + FlattenedMapWithOrder(np.ndarray array) except + + + cdef cppclass FlattenedMapWithStride "eigency::FlattenedMap" [DenseType, dtype, Rows, Cols, StorageOrder, MapOptions, StrideOuter, StrideInner]: + FlattenedMapWithStride() except + + FlattenedMapWithStride(np.ndarray array) except + + + cdef np.ndarray ndarray_view(PlainObjectBase &) + cdef np.ndarray ndarray_copy(PlainObjectBase &) + cdef np.ndarray ndarray(PlainObjectBase &) + + +cdef extern from "eigency_cpp.h" namespace "Eigen": + + cdef cppclass Dynamic: + pass + + cdef cppclass RowMajor: + pass + + cdef cppclass ColMajor: + pass + + cdef cppclass Aligned: + pass + + cdef cppclass Unaligned: + pass + + cdef cppclass Matrix(PlainObjectBase): + pass + + cdef cppclass Array(PlainObjectBase): + pass + + cdef cppclass VectorXd(PlainObjectBase): + pass + + cdef cppclass Vector1i(PlainObjectBase): + pass + + cdef cppclass Vector2i(PlainObjectBase): + pass + + cdef cppclass Vector3i(PlainObjectBase): + pass + + cdef cppclass Vector4i(PlainObjectBase): + pass + + cdef cppclass VectorXi(PlainObjectBase): + pass + + cdef cppclass RowVector1i(PlainObjectBase): + pass + + cdef cppclass RowVector2i(PlainObjectBase): + pass + + cdef cppclass RowVector3i(PlainObjectBase): + pass + + cdef cppclass RowVector4i(PlainObjectBase): + pass + + cdef cppclass RowVectorXi(PlainObjectBase): + pass + + cdef cppclass Matrix1i(PlainObjectBase): + pass + + cdef cppclass Matrix2i(PlainObjectBase): + pass + + cdef cppclass Matrix3i(PlainObjectBase): + pass + + cdef cppclass Matrix4i(PlainObjectBase): + pass + + cdef cppclass MatrixXi(PlainObjectBase): + pass + + cdef cppclass Vector1f(PlainObjectBase): + pass + + cdef cppclass Vector2f(PlainObjectBase): + pass + + cdef cppclass Vector3f(PlainObjectBase): + pass + + cdef cppclass Vector4f(PlainObjectBase): + pass + + cdef cppclass VectorXf(PlainObjectBase): + pass + + cdef cppclass RowVector1f(PlainObjectBase): + pass + + cdef cppclass RowVector2f(PlainObjectBase): + pass + + cdef cppclass RowVector3f(PlainObjectBase): + pass + + cdef cppclass RowVector4f(PlainObjectBase): + pass + + cdef cppclass RowVectorXf(PlainObjectBase): + pass + + cdef cppclass Matrix1f(PlainObjectBase): + pass + + cdef cppclass Matrix2f(PlainObjectBase): + pass + + cdef cppclass Matrix3f(PlainObjectBase): + pass + + cdef cppclass Matrix4f(PlainObjectBase): + pass + + cdef cppclass MatrixXf(PlainObjectBase): + pass + + cdef cppclass Vector1d(PlainObjectBase): + pass + + cdef cppclass Vector2d(PlainObjectBase): + pass + + cdef cppclass Vector3d(PlainObjectBase): + pass + + cdef cppclass Vector4d(PlainObjectBase): + pass + + cdef cppclass VectorXd(PlainObjectBase): + pass + + cdef cppclass RowVector1d(PlainObjectBase): + pass + + cdef cppclass RowVector2d(PlainObjectBase): + pass + + cdef cppclass RowVector3d(PlainObjectBase): + pass + + cdef cppclass RowVector4d(PlainObjectBase): + pass + + cdef cppclass RowVectorXd(PlainObjectBase): + pass + + cdef cppclass Matrix1d(PlainObjectBase): + pass + + cdef cppclass Matrix2d(PlainObjectBase): + pass + + cdef cppclass Matrix3d(PlainObjectBase): + pass + + cdef cppclass Matrix4d(PlainObjectBase): + pass + + cdef cppclass MatrixXd(PlainObjectBase): + pass + + cdef cppclass Vector1cf(PlainObjectBase): + pass + + cdef cppclass Vector2cf(PlainObjectBase): + pass + + cdef cppclass Vector3cf(PlainObjectBase): + pass + + cdef cppclass Vector4cf(PlainObjectBase): + pass + + cdef cppclass VectorXcf(PlainObjectBase): + pass + + cdef cppclass RowVector1cf(PlainObjectBase): + pass + + cdef cppclass RowVector2cf(PlainObjectBase): + pass + + cdef cppclass RowVector3cf(PlainObjectBase): + pass + + cdef cppclass RowVector4cf(PlainObjectBase): + pass + + cdef cppclass RowVectorXcf(PlainObjectBase): + pass + + cdef cppclass Matrix1cf(PlainObjectBase): + pass + + cdef cppclass Matrix2cf(PlainObjectBase): + pass + + cdef cppclass Matrix3cf(PlainObjectBase): + pass + + cdef cppclass Matrix4cf(PlainObjectBase): + pass + + cdef cppclass MatrixXcf(PlainObjectBase): + pass + + cdef cppclass Vector1cd(PlainObjectBase): + pass + + cdef cppclass Vector2cd(PlainObjectBase): + pass + + cdef cppclass Vector3cd(PlainObjectBase): + pass + + cdef cppclass Vector4cd(PlainObjectBase): + pass + + cdef cppclass VectorXcd(PlainObjectBase): + pass + + cdef cppclass RowVector1cd(PlainObjectBase): + pass + + cdef cppclass RowVector2cd(PlainObjectBase): + pass + + cdef cppclass RowVector3cd(PlainObjectBase): + pass + + cdef cppclass RowVector4cd(PlainObjectBase): + pass + + cdef cppclass RowVectorXcd(PlainObjectBase): + pass + + cdef cppclass Matrix1cd(PlainObjectBase): + pass + + cdef cppclass Matrix2cd(PlainObjectBase): + pass + + cdef cppclass Matrix3cd(PlainObjectBase): + pass + + cdef cppclass Matrix4cd(PlainObjectBase): + pass + + cdef cppclass MatrixXcd(PlainObjectBase): + pass + + cdef cppclass Array22i(PlainObjectBase): + pass + + cdef cppclass Array23i(PlainObjectBase): + pass + + cdef cppclass Array24i(PlainObjectBase): + pass + + cdef cppclass Array2Xi(PlainObjectBase): + pass + + cdef cppclass Array32i(PlainObjectBase): + pass + + cdef cppclass Array33i(PlainObjectBase): + pass + + cdef cppclass Array34i(PlainObjectBase): + pass + + cdef cppclass Array3Xi(PlainObjectBase): + pass + + cdef cppclass Array42i(PlainObjectBase): + pass + + cdef cppclass Array43i(PlainObjectBase): + pass + + cdef cppclass Array44i(PlainObjectBase): + pass + + cdef cppclass Array4Xi(PlainObjectBase): + pass + + cdef cppclass ArrayX2i(PlainObjectBase): + pass + + cdef cppclass ArrayX3i(PlainObjectBase): + pass + + cdef cppclass ArrayX4i(PlainObjectBase): + pass + + cdef cppclass ArrayXXi(PlainObjectBase): + pass + + cdef cppclass Array2i(PlainObjectBase): + pass + + cdef cppclass Array3i(PlainObjectBase): + pass + + cdef cppclass Array4i(PlainObjectBase): + pass + + cdef cppclass ArrayXi(PlainObjectBase): + pass + + cdef cppclass Array22f(PlainObjectBase): + pass + + cdef cppclass Array23f(PlainObjectBase): + pass + + cdef cppclass Array24f(PlainObjectBase): + pass + + cdef cppclass Array2Xf(PlainObjectBase): + pass + + cdef cppclass Array32f(PlainObjectBase): + pass + + cdef cppclass Array33f(PlainObjectBase): + pass + + cdef cppclass Array34f(PlainObjectBase): + pass + + cdef cppclass Array3Xf(PlainObjectBase): + pass + + cdef cppclass Array42f(PlainObjectBase): + pass + + cdef cppclass Array43f(PlainObjectBase): + pass + + cdef cppclass Array44f(PlainObjectBase): + pass + + cdef cppclass Array4Xf(PlainObjectBase): + pass + + cdef cppclass ArrayX2f(PlainObjectBase): + pass + + cdef cppclass ArrayX3f(PlainObjectBase): + pass + + cdef cppclass ArrayX4f(PlainObjectBase): + pass + + cdef cppclass ArrayXXf(PlainObjectBase): + pass + + cdef cppclass Array2f(PlainObjectBase): + pass + + cdef cppclass Array3f(PlainObjectBase): + pass + + cdef cppclass Array4f(PlainObjectBase): + pass + + cdef cppclass ArrayXf(PlainObjectBase): + pass + + cdef cppclass Array22d(PlainObjectBase): + pass + + cdef cppclass Array23d(PlainObjectBase): + pass + + cdef cppclass Array24d(PlainObjectBase): + pass + + cdef cppclass Array2Xd(PlainObjectBase): + pass + + cdef cppclass Array32d(PlainObjectBase): + pass + + cdef cppclass Array33d(PlainObjectBase): + pass + + cdef cppclass Array34d(PlainObjectBase): + pass + + cdef cppclass Array3Xd(PlainObjectBase): + pass + + cdef cppclass Array42d(PlainObjectBase): + pass + + cdef cppclass Array43d(PlainObjectBase): + pass + + cdef cppclass Array44d(PlainObjectBase): + pass + + cdef cppclass Array4Xd(PlainObjectBase): + pass + + cdef cppclass ArrayX2d(PlainObjectBase): + pass + + cdef cppclass ArrayX3d(PlainObjectBase): + pass + + cdef cppclass ArrayX4d(PlainObjectBase): + pass + + cdef cppclass ArrayXXd(PlainObjectBase): + pass + + cdef cppclass Array2d(PlainObjectBase): + pass + + cdef cppclass Array3d(PlainObjectBase): + pass + + cdef cppclass Array4d(PlainObjectBase): + pass + + cdef cppclass ArrayXd(PlainObjectBase): + pass + + cdef cppclass Array22cf(PlainObjectBase): + pass + + cdef cppclass Array23cf(PlainObjectBase): + pass + + cdef cppclass Array24cf(PlainObjectBase): + pass + + cdef cppclass Array2Xcf(PlainObjectBase): + pass + + cdef cppclass Array32cf(PlainObjectBase): + pass + + cdef cppclass Array33cf(PlainObjectBase): + pass + + cdef cppclass Array34cf(PlainObjectBase): + pass + + cdef cppclass Array3Xcf(PlainObjectBase): + pass + + cdef cppclass Array42cf(PlainObjectBase): + pass + + cdef cppclass Array43cf(PlainObjectBase): + pass + + cdef cppclass Array44cf(PlainObjectBase): + pass + + cdef cppclass Array4Xcf(PlainObjectBase): + pass + + cdef cppclass ArrayX2cf(PlainObjectBase): + pass + + cdef cppclass ArrayX3cf(PlainObjectBase): + pass + + cdef cppclass ArrayX4cf(PlainObjectBase): + pass + + cdef cppclass ArrayXXcf(PlainObjectBase): + pass + + cdef cppclass Array2cf(PlainObjectBase): + pass + + cdef cppclass Array3cf(PlainObjectBase): + pass + + cdef cppclass Array4cf(PlainObjectBase): + pass + + cdef cppclass ArrayXcf(PlainObjectBase): + pass + + cdef cppclass Array22cd(PlainObjectBase): + pass + + cdef cppclass Array23cd(PlainObjectBase): + pass + + cdef cppclass Array24cd(PlainObjectBase): + pass + + cdef cppclass Array2Xcd(PlainObjectBase): + pass + + cdef cppclass Array32cd(PlainObjectBase): + pass + + cdef cppclass Array33cd(PlainObjectBase): + pass + + cdef cppclass Array34cd(PlainObjectBase): + pass + + cdef cppclass Array3Xcd(PlainObjectBase): + pass + + cdef cppclass Array42cd(PlainObjectBase): + pass + + cdef cppclass Array43cd(PlainObjectBase): + pass + + cdef cppclass Array44cd(PlainObjectBase): + pass + + cdef cppclass Array4Xcd(PlainObjectBase): + pass + + cdef cppclass ArrayX2cd(PlainObjectBase): + pass + + cdef cppclass ArrayX3cd(PlainObjectBase): + pass + + cdef cppclass ArrayX4cd(PlainObjectBase): + pass + + cdef cppclass ArrayXXcd(PlainObjectBase): + pass + + cdef cppclass Array2cd(PlainObjectBase): + pass + + cdef cppclass Array3cd(PlainObjectBase): + pass + + cdef cppclass Array4cd(PlainObjectBase): + pass + + cdef cppclass ArrayXcd(PlainObjectBase): + pass + + diff --git a/cython/gtsam_eigency/core.pyx b/cython/gtsam_eigency/core.pyx new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/cython/gtsam_eigency/core.pyx @@ -0,0 +1 @@ + diff --git a/cython/gtsam_eigency/eigency_cpp.h b/cython/gtsam_eigency/eigency_cpp.h new file mode 100644 index 000000000..45ac6caaa --- /dev/null +++ b/cython/gtsam_eigency/eigency_cpp.h @@ -0,0 +1,452 @@ +#include + +#include +#include +#include + +typedef ::std::complex< double > __pyx_t_double_complex; +typedef ::std::complex< float > __pyx_t_float_complex; + +#include "conversions_api.h" + +#ifndef EIGENCY_CPP +#define EIGENCY_CPP + +namespace eigency { + +template +inline PyArrayObject *_ndarray_view(Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0); +template +inline PyArrayObject *_ndarray_copy(const Scalar *, long rows, long cols, bool is_row_major, long outer_stride=0, long inner_stride=0); + +// Strides: +// Eigen and numpy differ in their way of dealing with strides. Eigen has the concept of outer and +// inner strides, which are dependent on whether the array/matrix is row-major of column-major: +// Inner stride: denotes the offset between succeeding elements in each row (row-major) or column (column-major). +// Outer stride: denotes the offset between succeeding rows (row-major) or succeeding columns (column-major). +// In contrast, numpy's stride is simply a measure of how fast each dimension should be incremented. +// Consequently, a switch in numpy storage order from row-major to column-major involves a switch +// in strides, while it does not affect the stride in Eigen. +template<> +inline PyArrayObject *_ndarray_view(double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) { + // Eigen row-major mode: row_stride=outer_stride, and col_stride=inner_stride + // If no stride is given, the row_stride is set to the number of columns. + return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + } else { + // Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride + // If no stride is given, the cow_stride is set to the number of rows. + return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); + } +} +template<> +inline PyArrayObject *_ndarray_copy(const double *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const float *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_long_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_long_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned long *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_ulong_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_ulong_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_int_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_int_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned int *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_uint_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_uint_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_short_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_short_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned short *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_ushort_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_ushort_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const signed char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_schar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_schar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view(unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy(const unsigned char *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_uchar_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_uchar_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view >(std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy >(const std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_complex_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_complex_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + +template<> +inline PyArrayObject *_ndarray_view >(std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} +template<> +inline PyArrayObject *_ndarray_copy >(const std::complex *data, long rows, long cols, bool is_row_major, long outer_stride, long inner_stride) { + if (is_row_major) + return ndarray_copy_complex_float_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); + else + return ndarray_copy_complex_float_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); +} + + +template +inline PyArrayObject *ndarray(Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +// If C++11 is available, check if m is an r-value reference, in +// which case a copy should always be made +#if __cplusplus >= 201103L +template +inline PyArrayObject *ndarray(Eigen::PlainObjectBase &&m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +#endif +template +inline PyArrayObject *ndarray(const Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +template +inline PyArrayObject *ndarray_view(Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} +template +inline PyArrayObject *ndarray_view(const Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(const_cast(m.data()), m.rows(), m.cols(), m.IsRowMajor); +} +template +inline PyArrayObject *ndarray_copy(const Eigen::PlainObjectBase &m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor); +} + +template +inline PyArrayObject *ndarray(Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray(const Eigen::Map &m) { + import_gtsam_eigency__conversions(); + // Since this is a map, we assume that ownership is correctly taken care + // of, and we avoid taking a copy + return _ndarray_view(const_cast(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray_view(Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray_view(const Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_view(const_cast(m.data()), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} +template +inline PyArrayObject *ndarray_copy(const Eigen::Map &m) { + import_gtsam_eigency__conversions(); + return _ndarray_copy(m.data(), m.rows(), m.cols(), m.IsRowMajor, m.outerStride(), m.innerStride()); +} + + +template > +class MapBase: public Eigen::Map { +public: + typedef Eigen::Map Base; + typedef typename Base::Scalar Scalar; + + MapBase(Scalar* data, + long rows, + long cols, + _StrideType stride=_StrideType()) + : Base(data, + // If both dimensions are dynamic or dimensions match, accept dimensions as they are + ((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) || + (Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols)) + ? rows + // otherwise, test if swapping them makes them fit + : ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows) + ? cols + : rows), + ((Base::RowsAtCompileTime==Eigen::Dynamic && Base::ColsAtCompileTime==Eigen::Dynamic) || + (Base::RowsAtCompileTime==rows && Base::ColsAtCompileTime==cols)) + ? cols + : ((Base::RowsAtCompileTime==cols || Base::ColsAtCompileTime==rows) + ? rows + : cols), + stride + ) {} +}; + + +template class DenseBase, + typename Scalar, + int _Rows, int _Cols, + int _Options = Eigen::AutoAlign | +#if defined(__GNUC__) && __GNUC__==3 && __GNUC_MINOR__==4 + // workaround a bug in at least gcc 3.4.6 + // the innermost ?: ternary operator is misparsed. We write it slightly + // differently and this makes gcc 3.4.6 happy, but it's ugly. + // The error would only show up with EIGEN_DEFAULT_TO_ROW_MAJOR is defined + // (when EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION is RowMajor) + ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor +// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19 +#if EIGEN_VERSION_AT_LEAST(3,2,90) + : !(_Cols==1 && _Rows!=1) ? EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION +#else + : !(_Cols==1 && _Rows!=1) ? Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION +#endif + : ColMajor ), +#else + ( (_Rows==1 && _Cols!=1) ? Eigen::RowMajor + : (_Cols==1 && _Rows!=1) ? Eigen::ColMajor +// EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION contains explicit namespace since Eigen 3.1.19 +#if EIGEN_VERSION_AT_LEAST(3,2,90) + : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ), +#else + : Eigen::EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ), +#endif +#endif + int _MapOptions = Eigen::Unaligned, + int _StrideOuter=0, int _StrideInner=0, + int _MaxRows = _Rows, + int _MaxCols = _Cols> +class FlattenedMap: public MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > { +public: + typedef MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base; + + FlattenedMap() + : Base(NULL, 0, 0) {} + + FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0) + : Base(data, rows, cols, + Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)) { + } + + FlattenedMap(PyArrayObject *object) + : Base((Scalar *)((PyArrayObject*)object)->data, + // : Base(_from_numpy((PyArrayObject*)object), + (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, + (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0], + Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, + _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])) { + + if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object)) + throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); + } + FlattenedMap &operator=(const FlattenedMap &other) { + // Replace the memory that we point to (not a memory allocation) + new (this) FlattenedMap(const_cast(other.data()), + other.rows(), + other.cols(), + other.outerStride(), + other.innerStride()); + return *this; + } + + operator Base() const { + return static_cast(*this); + } + + operator Base&() const { + return static_cast(*this); + } + + operator DenseBase() const { + return DenseBase(static_cast(*this)); + } +}; + + +template +class Map: public MapBase { +public: + typedef MapBase Base; + typedef typename MatrixType::Scalar Scalar; + + Map() + : Base(NULL, 0, 0) { + } + + Map(Scalar *data, long rows, long cols) + : Base(data, rows, cols) {} + + Map(PyArrayObject *object) + : Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data, + // ROW: If array is in row-major order, transpose (see README) + (PyObject*)object == Py_None? 0 : + (PyArray_IS_C_CONTIGUOUS(object) + ? ((object->nd == 1) + ? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector) + : object->dimensions[1]) + : object->dimensions[0]), + // COLUMN: If array is in row-major order: transpose (see README) + (PyObject*)object == Py_None? 0 : + (PyArray_IS_C_CONTIGUOUS(object) + ? object->dimensions[0] + : ((object->nd == 1) + ? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector) + : object->dimensions[1]))) { + + if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object)) + throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); + } + + Map &operator=(const Map &other) { + // Replace the memory that we point to (not a memory allocation) + new (this) Map(const_cast(other.data()), + other.rows(), + other.cols()); + return *this; + } + + operator Base() const { + return static_cast(*this); + } + + operator Base&() const { + return static_cast(*this); + } + + operator MatrixType() const { + return MatrixType(static_cast(*this)); + } +}; + + +} + +#endif + + + diff --git a/gtsam.h b/gtsam.h index cb0d4791b..e679b6838 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2,9 +2,13 @@ * GTSAM Wrap Module Definition * - * These are the current classes available through the matlab toolbox interface, + * These are the current classes available through the matlab and python wrappers, * add more functions/classes as they are available. * + * IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the + * argument names matter. An implementation restriction is that in overloaded methods + * or functions, arguments of different types *have* to have different names. + * * Requirements: * Classes must start with an uppercase letter * - Can wrap a typedef @@ -16,7 +20,7 @@ * - Any class with which be copied with boost::make_shared() * - boost::shared_ptr of any object type * Constructors - * - Overloads are supported + * - Overloads are supported, but arguments of different types *have* to have different names * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB * Methods * - Constness has no effect @@ -26,7 +30,7 @@ * Static methods * - Must start with a letter (upper or lowercase) and use the "static" keyword * - The first letter will be made uppercase in the generated MATLAB interface - * - Overloads are supported + * - Overloads are supported, but arguments of different types *have* to have different names * Arguments to functions any of * - Eigen types: Matrix, Vector * - Eigen types and classes as an optionally const reference @@ -145,9 +149,9 @@ class KeyList { // Actually a FastSet class KeySet { KeySet(); - KeySet(const gtsam::KeySet& other); - KeySet(const gtsam::KeyVector& other); - KeySet(const gtsam::KeyList& other); + KeySet(const gtsam::KeySet& set); + KeySet(const gtsam::KeyVector& vector); + KeySet(const gtsam::KeyList& list); // Testable void print(string s) const; @@ -480,6 +484,11 @@ class Rot3 { // Standard Constructors and Named Constructors Rot3(); Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33); + static gtsam::Rot3 Rx(double t); static gtsam::Rot3 Ry(double t); static gtsam::Rot3 Rz(double t); @@ -491,6 +500,7 @@ class Rot3 { static gtsam::Rot3 Ypr(double y, double p, double r); static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); // Testable void print(string s) const; @@ -586,7 +596,7 @@ class Pose3 { Pose3(const gtsam::Pose3& other); Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); + Pose3(Matrix mat); // Testable void print(string s) const; @@ -595,23 +605,23 @@ class Pose3 { // Group static gtsam::Pose3 identity(); gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; // Manifold gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; // Lie Group static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); + static Vector Logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; + gtsam::Point3 transform_from(const gtsam::Point3& point) const; + gtsam::Point3 transform_to(const gtsam::Point3& point) const; // Standard Interface gtsam::Rot3 rotation() const; @@ -636,7 +646,7 @@ class Pose3Vector size_t size() const; bool empty() const; gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& x); + void push_back(const gtsam::Pose3& pose); }; #include @@ -909,7 +919,7 @@ class PinholeCamera { gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); + double range(const gtsam::Pose3& pose); // enabling serialization functionality void serialize() const; @@ -946,7 +956,7 @@ virtual class SimpleCamera { gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); + double range(const gtsam::Pose3& pose); // enabling serialization functionality void serialize() const; @@ -1060,13 +1070,13 @@ virtual class SymbolicFactorGraph { const gtsam::Ordering& ordering); pair eliminatePartialMultifrontal( const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); }; #include @@ -1164,9 +1174,9 @@ class VariableIndex { //template //VariableIndex(const T& factorGraph, size_t nVariables); //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); - VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); - VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& sfg); + VariableIndex(const gtsam::GaussianFactorGraph& gfg); + VariableIndex(const gtsam::NonlinearFactorGraph& fg); VariableIndex(const gtsam::VariableIndex& other); // Testable @@ -1460,7 +1470,7 @@ class GaussianFactorGraph { // Building the graph void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* factor); + void push_back(const gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianFactorGraph& graph); void push_back(const gtsam::GaussianBayesNet& bayesNet); void push_back(const gtsam::GaussianBayesTree& bayesTree); @@ -1499,13 +1509,13 @@ class GaussianFactorGraph { const gtsam::Ordering& ordering); pair eliminatePartialMultifrontal( const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); // Conversion to matrices Matrix sparseJacobian_() const; @@ -1859,34 +1869,34 @@ class Values { // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; - void insert(size_t j, const gtsam::Point2& t); - void insert(size_t j, const gtsam::Point3& t); - void insert(size_t j, const gtsam::Rot2& t); - void insert(size_t j, const gtsam::Pose2& t); - void insert(size_t j, const gtsam::Rot3& t); - void insert(size_t j, const gtsam::Pose3& t); - void insert(size_t j, const gtsam::Cal3_S2& t); - void insert(size_t j, const gtsam::Cal3DS2& t); - void insert(size_t j, const gtsam::Cal3Bundler& t); - void insert(size_t j, const gtsam::EssentialMatrix& t); - void insert(size_t j, const gtsam::SimpleCamera& t); - void insert(size_t j, const gtsam::imuBias::ConstantBias& t); - void insert(size_t j, Vector t); - void insert(size_t j, Matrix t); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::SimpleCamera& simpel_camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); - void update(size_t j, const gtsam::Point2& t); - void update(size_t j, const gtsam::Point3& t); - void update(size_t j, const gtsam::Rot2& t); - void update(size_t j, const gtsam::Pose2& t); - void update(size_t j, const gtsam::Rot3& t); - void update(size_t j, const gtsam::Pose3& t); - void update(size_t j, const gtsam::Cal3_S2& t); - void update(size_t j, const gtsam::Cal3DS2& t); - void update(size_t j, const gtsam::Cal3Bundler& t); - void update(size_t j, const gtsam::EssentialMatrix& t); - void update(size_t j, const gtsam::imuBias::ConstantBias& t); - void update(size_t j, Vector t); - void update(size_t j, Matrix t); + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, Vector vector); + void update(size_t j, Matrix matrix); template T at(size_t j); @@ -2100,10 +2110,10 @@ class ISAM2Params { void print(string str) const; /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); - void setRelinearizeThreshold(double relinearizeThreshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); int getRelinearizeSkip() const; void setRelinearizeSkip(int relinearizeSkip); bool isEnableRelinearization() const; @@ -2664,4 +2674,10 @@ namespace utilities { } //\namespace utilities +#include +class RedirectCout { + RedirectCout(); + string str(); +}; + } diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 891c6fd80..3816f26f8 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -249,6 +250,32 @@ Values localToWorld(const Values& local, const Pose2& base, return world; } -} +} // namespace utilities + +/** + * For Python __str__(). + * Redirect std cout to a string stream so we can return a string representation + * of an object when it prints to cout. + * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string + */ +struct RedirectCout { + /// constructor -- redirect stdout buffer to a stringstream buffer + RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} + + /// return the string + std::string str() const { + return ssBuffer_.str(); + } + + /// destructor -- redirect stdout buffer to its original buffer + ~RedirectCout() { + std::cout.rdbuf(coutBuffer_); + } + +private: + std::stringstream ssBuffer_; + std::streambuf* coutBuffer_; +}; + } diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 781b08d57..a1a8e3a90 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -20,3 +20,7 @@ list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") if("@GTSAM_USE_EIGEN_MKL@") list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") endif() + +if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") + list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") +endif() diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 16cce45c2..39c910826 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -103,7 +103,7 @@ class PoseRTV { #include class Pose3Upright { Pose3Upright(); - Pose3Upright(const gtsam::Pose3Upright& x); + Pose3Upright(const gtsam::Pose3Upright& other); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); Pose3Upright(double x, double y, double z, double theta); Pose3Upright(const gtsam::Pose2& pose, double z); @@ -141,7 +141,7 @@ class Pose3Upright { #include class BearingS2 { BearingS2(); - BearingS2(double azimuth, double elevation); + BearingS2(double azimuth_double, double elevation_double); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); gtsam::Rot2 azimuth() const; diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 6455297c4..913c11284 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -281,15 +281,16 @@ std::string ArgumentList::pyx_paramsList() const { } /* ************************************************************************* */ -std::string ArgumentList::pyx_castParamsToPythonType() const { - if (size() == 0) - return " pass\n"; +std::string ArgumentList::pyx_castParamsToPythonType( + const std::string& indent) const { + if (size() == 0) + return ""; // cast params to their correct python argument type to pass in the function call later string s; for (size_t j = 0; j < size(); ++j) - s += " " + at(j).name + " = <" + at(j).type.pyxArgumentType() - + ">(__params['" + at(j).name + "'])\n"; + s += indent + at(j).name + " = <" + at(j).type.pyxArgumentType() + + ">(__params[" + std::to_string(j) + "])\n"; return s; } diff --git a/wrap/Argument.h b/wrap/Argument.h index c06e4f797..0a4ebba9d 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -131,7 +131,7 @@ struct ArgumentList: public std::vector { void emit_cython_pyx(FileWriter& file) const; std::string pyx_asParams() const; std::string pyx_paramsList() const; - std::string pyx_castParamsToPythonType() const; + std::string pyx_castParamsToPythonType(const std::string& indent) const; std::string pyx_convertEigenTypeAndStorageOrder(const std::string& indent) const; /** diff --git a/wrap/Class.cpp b/wrap/Class.cpp index a2e42cb1c..bff1d36b2 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -764,6 +764,22 @@ void Class::emit_cython_pxd(FileWriter& pxdFile) const { if (numMethods == 0) pxdFile.oss << " pass\n"; } +/* ************************************************************************* */ +void Class::emit_cython_wrapper_pxd(FileWriter& pxdFile) const { + pxdFile.oss << "\ncdef class " << pyxClassName(); + if (getParent()) + pxdFile.oss << "(" << getParent()->pyxClassName() << ")"; + pxdFile.oss << ":\n"; + pxdFile.oss << " cdef " << shared_pxd_class_in_pyx() << " " + << shared_pxd_obj_in_pyx() << "\n"; + // cyCreateFromShared + pxdFile.oss << " @staticmethod\n"; + pxdFile.oss << " cdef " << pyxClassName() << " cyCreateFromShared(const " + << shared_pxd_class_in_pyx() << "& other)\n"; + for(const StaticMethod& m: static_methods | boost::adaptors::map_values) + m.emit_cython_wrapper_pxd(pxdFile, *this); + if (static_methods.size()>0) pxdFile.oss << "\n"; +} /* ************************************************************************* */ void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, @@ -825,40 +841,30 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector& allCl if (parentClass) pyxFile.oss << "(" << parentClass->pyxClassName() << ")"; pyxFile.oss << ":\n"; - // shared variable of the corresponding cython object - // pyxFile.oss << " cdef " << shared_pxd_class_in_pyx() << " " << shared_pxd_obj_in_pyx() << "\n"; - - // __cinit___ - pyxFile.oss << " def __init__(self, *args, **kwargs):\n" - " self." << shared_pxd_obj_in_pyx() << " = " - << shared_pxd_class_in_pyx() << "()\n"; - + // __init___ + pyxFile.oss << " def __init__(self, *args, **kwargs):\n"; + pyxFile.oss << " cdef list __params\n"; + pyxFile.oss << " self." << shared_pxd_obj_in_pyx() << " = " << shared_pxd_class_in_pyx() << "()\n"; pyxFile.oss << " if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):\n return\n"; - for (size_t i = 0; i0) pyxFile.oss << "\n"; + pyxFile.oss << " if (self." << shared_pxd_obj_in_pyx() << ".use_count()==0):\n"; + pyxFile.oss << " raise TypeError('" << pyxClassName() + << " construction failed!')\n"; + pyxInitParentObj(pyxFile, " self", "self." + shared_pxd_obj_in_pyx(), allClasses); + pyxFile.oss << "\n"; // cyCreateFromShared pyxFile.oss << " @staticmethod\n"; - pyxFile.oss << " cdef " << pyxClassName() << " cyCreateFromShared(const " + pyxFile.oss << " cdef " << pyxClassName() << " cyCreateFromShared(const " << shared_pxd_class_in_pyx() << "& other):\n" << " if other.get() == NULL:\n" << " raise RuntimeError('Cannot create object from a nullptr!')\n" - << " cdef " << pyxClassName() << " ret = " << pyxClassName() << "(cyCreateFromShared=True)\n" - << " ret." << shared_pxd_obj_in_pyx() << " = other\n"; - pyxInitParentObj(pyxFile, " ret", "other", allClasses); - pyxFile.oss << " return ret" << "\n"; + << " cdef " << pyxClassName() << " return_value = " << pyxClassName() << "(cyCreateFromShared=True)\n" + << " return_value." << shared_pxd_obj_in_pyx() << " = other\n"; + pyxInitParentObj(pyxFile, " return_value", "other", allClasses); + pyxFile.oss << " return return_value" << "\n\n"; for(const StaticMethod& m: static_methods | boost::adaptors::map_values) m.emit_cython_pyx(pyxFile, *this); @@ -869,7 +875,7 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector& allCl pyxDynamicCast(pyxFile, *this, allClasses); - pyxFile.oss << "\n\n"; + pyxFile.oss << "\n\n"; } /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index 1a8092228..910ecde57 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -165,6 +165,7 @@ public: // emit cython wrapper void emit_cython_pxd(FileWriter& pxdFile) const; + void emit_cython_wrapper_pxd(FileWriter& pxdFile) const; void emit_cython_pyx(FileWriter& pyxFile, const std::vector& allClasses) const; void pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj, diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index b2ebbce4c..1c9f83df8 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -136,8 +136,7 @@ void Constructor::emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const { // generate the constructor pxdFile.oss << " " << cls.pxdClassName() << "("; args.emit_cython_pxd(pxdFile, cls.pxdClassName(), cls.templateArgs); - pxdFile.oss << ") " - << "except +\n"; + pxdFile.oss << ") " << "except +\n"; } } @@ -145,15 +144,16 @@ void Constructor::emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const { void Constructor::emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const { for (size_t i = 0; i < nrOverloads(); i++) { ArgumentList args = argumentList(i); - pyxFile.oss << " def " + cls.pyxClassName() + "_" + to_string(i) + - "(self, *args, **kwargs):\n"; - pyxFile.oss << pyx_resolveOverloadParams(args, true); - pyxFile.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); + pyxFile.oss << " try:\n"; + pyxFile.oss << pyx_resolveOverloadParams(args, true, 3); + pyxFile.oss + << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); - pyxFile.oss << " self." << cls.shared_pxd_obj_in_pyx() << " = " - << cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx() - << "(" << args.pyx_asParams() << "))\n"; - pyxFile.oss << " return True\n\n"; + pyxFile.oss << " self." << cls.shared_pxd_obj_in_pyx() << " = " + << cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx() + << "(" << args.pyx_asParams() << "))\n"; + pyxFile.oss << " except:\n"; + pyxFile.oss << " pass\n"; } } diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index ff5c32d65..c8482f2c4 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -155,9 +155,11 @@ void GlobalFunction::emit_cython_pyx_no_overload(FileWriter& file) const { // Function definition file.oss << "def " << funcName; + // modify name of function instantiation as python doesn't allow overloads // e.g. template funcName(...) --> funcNameA, funcNameB, funcNameC if (templateArgValue_) file.oss << templateArgValue_->pyxClassName(); + // funtion arguments file.oss << "("; argumentList(0).emit_cython_pyx(file); @@ -189,28 +191,33 @@ void GlobalFunction::emit_cython_pyx(FileWriter& file) const { file.oss << "def " << funcName << "(*args, **kwargs):\n"; for (size_t i = 0; i < N; ++i) { file.oss << " success, results = " << funcName << "_" << i - << "(*args, **kwargs)\n"; + << "(args, kwargs)\n"; file.oss << " if success:\n return results\n"; } file.oss << " raise TypeError('Could not find the correct overload')\n"; for (size_t i = 0; i < N; ++i) { ArgumentList args = argumentList(i); - file.oss << "def " + funcName + "_" + to_string(i) + - "(*args, **kwargs):\n"; - file.oss << pyx_resolveOverloadParams(args, false, 1); // lazy: always return None even if it's a void function - - /// Call cython corresponding function - file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); - string ret = pyx_functionCall("", pxdName(), i); + file.oss << "def " + funcName + "_" + to_string(i) + "(args, kwargs):\n"; + file.oss << " cdef list __params\n"; if (!returnVals_[i].isVoid()) { - file.oss << " cdef " << returnVals_[i].pyx_returnType() - << " ret = " << ret << "\n"; - file.oss << " return True, " << returnVals_[i].pyx_casting("ret") << "\n"; - } else { - file.oss << " " << ret << "\n"; - file.oss << " return True, None\n"; + file.oss << " cdef " << returnVals_[i].pyx_returnType() << " return_value\n"; } + file.oss << " try:\n"; + file.oss << pyx_resolveOverloadParams(args, false, 2); // lazy: always return None even if it's a void function + + /// Call corresponding cython function + file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); + string call = pyx_functionCall("", pxdName(), i); + if (!returnVals_[i].isVoid()) { + file.oss << " return_value = " << call << "\n"; + file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n"; + } else { + file.oss << " " << call << "\n"; + file.oss << " return True, None\n"; + } + file.oss << " except:\n"; + file.oss << " return False, None\n\n"; } } /* ************************************************************************* */ diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 9670fb090..d230dee51 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -85,8 +85,12 @@ void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const { for (size_t i = 0; i < nrOverloads(); ++i) { file.oss << " "; returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs); - file.oss << pyRename(name_) + " \"" + name_ + "\"" - << "("; + const string renamed = pyRename(name_); + if (renamed != name_) { + file.oss << pyRename(name_) + " \"" + name_ + "\"" << "("; + } else { + file.oss << name_ << "("; + } argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs); file.oss << ")"; // if (is_const_) file.oss << " const"; @@ -99,16 +103,23 @@ void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const { void Method::emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const { string funcName = pyRename(name_); + // leverage python's special treatment for print - if (funcName == "print_") - file.oss << " def __str__(self):\n self.print_('')\n return ''\n"; + if (funcName == "print_") { + file.oss << " def __str__(self):\n"; + file.oss << " strBuf = RedirectCout()\n"; + file.oss << " self.print_('')\n"; + file.oss << " return strBuf.str()\n"; + } // Function definition file.oss << " def " << funcName; + // modify name of function instantiation as python doesn't allow overloads // e.g. template funcName(...) --> funcNameA, funcNameB, funcNameC if (templateArgValue_) file.oss << templateArgValue_->pyxClassName(); - // funtion arguments + + // function arguments file.oss << "(self"; if (argumentList(0).size() > 0) file.oss << ", "; argumentList(0).emit_cython_pyx(file); @@ -134,7 +145,8 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const { // doesn't allow overloads // e.g. template funcName(...) --> funcNameA, funcNameB, funcNameC string instantiatedName = - (templateArgValue_) ? funcName + templateArgValue_->pyxClassName() : funcName; + (templateArgValue_) ? funcName + templateArgValue_->pyxClassName() : + funcName; size_t N = nrOverloads(); // It's easy if there's no overload @@ -145,32 +157,49 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const { // Dealing with overloads.. file.oss << " def " << instantiatedName << "(self, *args, **kwargs):\n"; - for (size_t i = 0; i < N; ++i) { - file.oss << " success, results = self." << instantiatedName << "_" << i - << "(*args, **kwargs)\n"; - file.oss << " if success:\n return results\n"; - } - file.oss << " raise TypeError('Could not find the correct overload')\n"; + file.oss << " cdef list __params\n"; - for (size_t i = 0; i < N; ++i) { - ArgumentList args = argumentList(i); - file.oss << " def " + instantiatedName + "_" + to_string(i) + - "(self, *args, **kwargs):\n"; - file.oss << pyx_resolveOverloadParams(args, false); // lazy: always return None even if it's a void function - - /// Call cython corresponding function - file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); - string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()"; - - string ret = pyx_functionCall(caller, funcName, i); - if (!returnVals_[i].isVoid()) { - file.oss << " cdef " << returnVals_[i].pyx_returnType() - << " ret = " << ret << "\n"; - file.oss << " return True, " << returnVals_[i].pyx_casting("ret") << "\n"; + // Define return values for all possible overloads + vector return_type; // every overload has a return type, possibly void + map return_value; // we only define one return value for every distinct type + size_t j = 1; + for (size_t i = 0; i < nrOverloads(); ++i) { + if (returnVals_[i].isVoid()) { + return_type.push_back("void"); } else { - file.oss << " " << ret << "\n"; - file.oss << " return True, None\n"; + const string type = returnVals_[i].pyx_returnType(); + return_type.push_back(type); + if (return_value.count(type) == 0) { + const string value = "return_value_" + to_string(j++); + return_value[type] = value; + file.oss << " cdef " << type << " " << value << "\n"; + } } } + + for (size_t i = 0; i < nrOverloads(); ++i) { + ArgumentList args = argumentList(i); + file.oss << " try:\n"; + file.oss << pyx_resolveOverloadParams(args, false, 3); // lazy: always return None even if it's a void function + + /// Call corresponding cython function + file.oss << args.pyx_convertEigenTypeAndStorageOrder(" "); + string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()"; + string call = pyx_functionCall(caller, funcName, i); + if (!returnVals_[i].isVoid()) { + const string type = return_type[i]; + const string value = return_value[type]; + file.oss << " " << value << " = " << call << "\n"; + file.oss << " return " << returnVals_[i].pyx_casting(value) + << "\n"; + } else { + file.oss << " " << call << "\n"; + file.oss << " return\n"; + } + file.oss << " except:\n"; + file.oss << " pass\n"; + } + file.oss + << " raise TypeError('Incorrect arguments for method call.')\n\n"; } /* ************************************************************************* */ diff --git a/wrap/Module.cpp b/wrap/Module.cpp index d071a5e2e..9eee686cb 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -334,7 +334,7 @@ void Module::generate_cython_wrapper(const string& toolboxPath, const std::strin /* ************************************************************************* */ void Module::emit_cython_pxd(FileWriter& pxdFile) const { // headers - pxdFile.oss << "from eigency.core cimport *\n" + pxdFile.oss << "from gtsam_eigency.core cimport *\n" "from libcpp.string cimport string\n" "from libcpp.vector cimport vector\n" "from libcpp.pair cimport pair\n" @@ -348,50 +348,41 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const { " shared_ptr()\n" " shared_ptr(T*)\n" " T* get()\n" + " long use_count() const\n" " T& operator*()\n\n" " cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n" - " cdef shared_ptr[T] make_shared[T](const T& r)\n"; + " cdef shared_ptr[T] make_shared[T](const T& r)\n\n"; for(const TypedefPair& types: typedefs) types.emit_cython_pxd(pxdFile); //... wrap all classes for (const Class& cls : uninstantiatedClasses) { - cls.emit_cython_pxd(pxdFile); + cls.emit_cython_pxd(pxdFile); - for (const Class& expCls : expandedClasses) { - //... ctypedef for template instantiations - if (expCls.templateClass && - expCls.templateClass->pxdClassName() == cls.pxdClassName()) { - pxdFile.oss << "ctypedef " << expCls.templateClass->pxdClassName() - << "["; - for (size_t i = 0; i < expCls.templateInstTypeList.size(); ++i) - pxdFile.oss << expCls.templateInstTypeList[i].pxdClassName() - << ((i == expCls.templateInstTypeList.size() - 1) - ? "" - : ", "); - pxdFile.oss << "] " << expCls.pxdClassName() << "\n"; - } + for (const Class& expCls : expandedClasses) { + bool matchingNonTemplated = !expCls.templateClass + && expCls.pxdClassName() == cls.pxdClassName(); + bool isTemplatedFromCls = expCls.templateClass + && expCls.templateClass->pxdClassName() == cls.pxdClassName(); - if ((!expCls.templateClass && - expCls.pxdClassName() == cls.pxdClassName()) || - (expCls.templateClass && - expCls.templateClass->pxdClassName() == cls.pxdClassName())) { - pxdFile.oss << "\n"; - pxdFile.oss << "cdef class " << expCls.pyxClassName(); - if (expCls.getParent()) - pxdFile.oss << "(" << expCls.getParent()->pyxClassName() << ")"; - pxdFile.oss << ":\n"; - pxdFile.oss << " cdef " << expCls.shared_pxd_class_in_pyx() - << " " << expCls.shared_pxd_obj_in_pyx() << "\n"; - // cyCreateFromShared - pxdFile.oss << " @staticmethod\n"; - pxdFile.oss << " cdef " << expCls.pyxClassName() - << " cyCreateFromShared(const " - << expCls.shared_pxd_class_in_pyx() << "& other)\n"; - } + // ctypedef for template instantiations + if (isTemplatedFromCls) { + pxdFile.oss << "\n"; + pxdFile.oss << "ctypedef " << expCls.templateClass->pxdClassName() + << "["; + for (size_t i = 0; i < expCls.templateInstTypeList.size(); ++i) + pxdFile.oss << expCls.templateInstTypeList[i].pxdClassName() + << ((i == expCls.templateInstTypeList.size() - 1) ? "" : ", "); + pxdFile.oss << "] " << expCls.pxdClassName() << "\n"; } - pxdFile.oss << "\n\n"; + + // Python wrapper class + if (isTemplatedFromCls || matchingNonTemplated) { + expCls.emit_cython_wrapper_pxd(pxdFile); + } + } + pxdFile.oss << "\n\n"; } //... wrap global functions @@ -412,11 +403,22 @@ void Module::emit_cython_pyx(FileWriter& pyxFile) const { "from "<< pxdHeader << " cimport dynamic_pointer_cast\n" "from "<< pxdHeader << " cimport make_shared\n"; + pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n" + "cdef list process_args(list keywords, tuple args, dict kwargs):\n" + " cdef str keyword\n" + " cdef int n = len(args), m = len(keywords)\n" + " cdef list params = list(args)\n" + " assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m)\n" + " try:\n" + " return params + [kwargs[keyword] for keyword in keywords[n:]]\n" + " except:\n" + " raise ValueError('Epected arguments ' + str(keywords))\n"; + // import all typedefs, e.g. from gtsam_wrapper cimport Key, so we don't need to say gtsam.Key for(const Qualified& q: Qualified::BasicTypedefs) { pyxFile.oss << "from " << pxdHeader << " cimport " << q.pxdClassName() << "\n"; } - pyxFile.oss << "from eigency.core cimport *\n" + pyxFile.oss << "from gtsam_eigency.core cimport *\n" "from libcpp cimport bool\n\n" "from libcpp.pair cimport pair\n" "from libcpp.string cimport string\n" diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 71acf822b..6bcb72d94 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -71,35 +71,29 @@ public: return os; } - std::string pyx_resolveOverloadParams(const ArgumentList& args, bool isVoid, size_t indentLevel = 2) const { + std::string pyx_resolveOverloadParams(const ArgumentList& args, bool isVoid, + size_t indentLevel = 2) const { std::string indent; - for (size_t i = 0; i 0) { - s += indent + "__params = kwargs.copy()\n"; - s += indent + "__names = [" + args.pyx_paramsList() + "]\n"; - s += indent + "for i in range(len(args)):\n"; - s += indent + " __params[__names[i]] = args[i]\n"; - for (size_t i = 0; i 1) { + for (size_t i = 0; i < nrOverloads(); ++i) { + string funcName = name_ + "_" + to_string(i); + file.oss << " @staticmethod\n"; + file.oss << " cdef tuple " + funcName + "(tuple args, dict kwargs)\n"; + } + } +} + /* ************************************************************************* */ void StaticMethod::emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const { @@ -80,12 +91,12 @@ void StaticMethod::emit_cython_pyx_no_overload(FileWriter& file, /// Call cython corresponding function and return file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder(" "); - string ret = pyx_functionCall(cls.pxd_class_in_pyx(), name_, 0); + string call = pyx_functionCall(cls.pxd_class_in_pyx(), name_, 0); file.oss << " "; if (!returnVals_[0].isVoid()) { - file.oss << "return " << returnVals_[0].pyx_casting(ret) << "\n"; + file.oss << "return " << returnVals_[0].pyx_casting(call) << "\n"; } else - file.oss << ret << "\n"; + file.oss << call << "\n"; file.oss << "\n"; } @@ -98,38 +109,42 @@ void StaticMethod::emit_cython_pyx(FileWriter& file, const Class& cls) const { } // Dealing with overloads.. - file.oss << " @staticmethod\n"; + file.oss << " @staticmethod # overloaded\n"; file.oss << " def " << name_ << "(*args, **kwargs):\n"; for (size_t i = 0; i < N; ++i) { string funcName = name_ + "_" + to_string(i); file.oss << " success, results = " << cls.pyxClassName() << "." - << funcName << "(*args, **kwargs)\n"; + << funcName << "(args, kwargs)\n"; file.oss << " if success:\n return results\n"; } - file.oss << " raise TypeError('Could not find the correct overload')\n"; + file.oss << " raise TypeError('Could not find the correct overload')\n\n"; + // Create cdef methods for all overloaded methods for(size_t i = 0; i < N; ++i) { - file.oss << " @staticmethod\n"; - string funcName = name_ + "_" + to_string(i); - string pxdFuncName = name_ + ((i>0)?"_" + to_string(i):""); + file.oss << " @staticmethod\n"; + file.oss << " cdef tuple " + funcName + "(tuple args, dict kwargs):\n"; + file.oss << " cdef list __params\n"; + if (!returnVals_[i].isVoid()) { + file.oss << " cdef " << returnVals_[i].pyx_returnType() << " return_value\n"; + } + file.oss << " try:\n"; ArgumentList args = argumentList(i); - file.oss << " def " + funcName + "(*args, **kwargs):\n"; - file.oss << pyx_resolveOverloadParams(args, false); // lazy: always return None even if it's a void function + file.oss << pyx_resolveOverloadParams(args, false, 3); /// Call cython corresponding function and return - file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" "); - string ret = pyx_functionCall(cls.pxd_class_in_pyx(), pxdFuncName, i); + file.oss << args.pyx_convertEigenTypeAndStorageOrder(" "); + string pxdFuncName = name_ + ((i>0)?"_" + to_string(i):""); + string call = pyx_functionCall(cls.pxd_class_in_pyx(), pxdFuncName, i); if (!returnVals_[i].isVoid()) { - file.oss << " cdef " << returnVals_[i].pyx_returnType() - << " ret = " << ret << "\n"; - file.oss << " return True, " << returnVals_[i].pyx_casting("ret") << "\n"; + file.oss << " return_value = " << call << "\n"; + file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n"; + } else { + file.oss << " " << call << "\n"; + file.oss << " return True, None\n"; } - else { - file.oss << " " << ret << "\n"; - file.oss << " return True, None\n"; - } - file.oss << "\n"; + file.oss << " except:\n"; + file.oss << " return False, None\n\n"; } } diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 2a45c107e..cbcfc8d49 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -35,6 +35,7 @@ struct StaticMethod: public MethodBase { } void emit_cython_pxd(FileWriter& file, const Class& cls) const; + void emit_cython_wrapper_pxd(FileWriter& file, const Class& cls) const; void emit_cython_pyx(FileWriter& file, const Class& cls) const; void emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const; diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd index 918eeea6f..f2cd513e2 100644 --- a/wrap/tests/expected-cython/geometry.pxd +++ b/wrap/tests/expected-cython/geometry.pxd @@ -1,5 +1,5 @@ -from eigency.core cimport * +from gtsam_eigency.core cimport * from libcpp.string cimport string from libcpp.vector cimport vector from libcpp.pair cimport pair @@ -12,26 +12,28 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost": shared_ptr() shared_ptr(T*) T* get() + long use_count() const T& operator*() cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r) cdef shared_ptr[T] make_shared[T](const T& r) + cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam": cdef cppclass CPoint2 "gtsam::Point2": CPoint2() except + CPoint2(double x, double y) except + - void argChar "argChar"(char a) except + - void argUChar "argUChar"(unsigned char a) except + - int dim "dim"() except + - void eigenArguments "eigenArguments"(const VectorXd& v, const MatrixXd& m) except + - char returnChar "returnChar"() except + - CVectorNotEigen vectorConfusion "vectorConfusion"() except + - double x "x"() except + - double y "y"() except + + void argChar(char a) except + + void argUChar(unsigned char a) except + + int dim() except + + void eigenArguments(const VectorXd& v, const MatrixXd& m) except + + char returnChar() except + + CVectorNotEigen vectorConfusion() except + + double x() except + + double y() except + cdef class Point2: - cdef shared_ptr[CPoint2] shared_CPoint2_ + cdef shared_ptr[CPoint2] CPoint2_ @staticmethod cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other) @@ -45,41 +47,42 @@ cdef extern from "gtsam/geometry/Point3.h" namespace "gtsam": @staticmethod double staticFunction "staticFunction"() except + - double norm "norm"() except + + double norm() except + cdef class Point3: - cdef shared_ptr[CPoint3] shared_CPoint3_ + cdef shared_ptr[CPoint3] CPoint3_ @staticmethod cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other) + cdef extern from "folder/path/to/Test.h": cdef cppclass CTest "Test": CTest() except + CTest(double a, const MatrixXd& b) except + - void arg_EigenConstRef "arg_EigenConstRef"(const MatrixXd& value) except + - pair[CTest,shared_ptr[CTest]] create_MixedPtrs "create_MixedPtrs"() except + - pair[shared_ptr[CTest],shared_ptr[CTest]] create_ptrs "create_ptrs"() except + + void arg_EigenConstRef(const MatrixXd& value) except + + pair[CTest,shared_ptr[CTest]] create_MixedPtrs() except + + pair[shared_ptr[CTest],shared_ptr[CTest]] create_ptrs() except + void print_ "print"() except + - shared_ptr[CPoint2] return_Point2Ptr "return_Point2Ptr"(bool value) except + - CTest return_Test "return_Test"(shared_ptr[CTest]& value) except + - shared_ptr[CTest] return_TestPtr "return_TestPtr"(shared_ptr[CTest]& value) except + - bool return_bool "return_bool"(bool value) except + - double return_double "return_double"(double value) except + - bool return_field "return_field"(const CTest& t) except + - int return_int "return_int"(int value) except + - MatrixXd return_matrix1 "return_matrix1"(const MatrixXd& value) except + - MatrixXd return_matrix2 "return_matrix2"(const MatrixXd& value) except + - pair[VectorXd,MatrixXd] return_pair "return_pair"(const VectorXd& v, const MatrixXd& A) except + - pair[shared_ptr[CTest],shared_ptr[CTest]] return_ptrs "return_ptrs"(shared_ptr[CTest]& p1, shared_ptr[CTest]& p2) except + - size_t return_size_t "return_size_t"(size_t value) except + - string return_string "return_string"(string value) except + - VectorXd return_vector1 "return_vector1"(const VectorXd& value) except + - VectorXd return_vector2 "return_vector2"(const VectorXd& value) except + + shared_ptr[CPoint2] return_Point2Ptr(bool value) except + + CTest return_Test(shared_ptr[CTest]& value) except + + shared_ptr[CTest] return_TestPtr(shared_ptr[CTest]& value) except + + bool return_bool(bool value) except + + double return_double(double value) except + + bool return_field(const CTest& t) except + + int return_int(int value) except + + MatrixXd return_matrix1(const MatrixXd& value) except + + MatrixXd return_matrix2(const MatrixXd& value) except + + pair[VectorXd,MatrixXd] return_pair(const VectorXd& v, const MatrixXd& A) except + + pair[shared_ptr[CTest],shared_ptr[CTest]] return_ptrs(shared_ptr[CTest]& p1, shared_ptr[CTest]& p2) except + + size_t return_size_t(size_t value) except + + string return_string(string value) except + + VectorXd return_vector1(const VectorXd& value) except + + VectorXd return_vector2(const VectorXd& value) except + cdef class Test: - cdef shared_ptr[CTest] shared_CTest_ + cdef shared_ptr[CTest] CTest_ @staticmethod cdef Test cyCreateFromShared(const shared_ptr[CTest]& other) @@ -89,7 +92,7 @@ cdef extern from "folder/path/to/Test.h": pass cdef class MyBase: - cdef shared_ptr[CMyBase] shared_CMyBase_ + cdef shared_ptr[CMyBase] CMyBase_ @staticmethod cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other) @@ -98,24 +101,26 @@ cdef extern from "folder/path/to/Test.h": cdef cppclass CMyTemplate "MyTemplate"[T](CMyBase): CMyTemplate() except + - void accept_T "accept_T"(const T& value) except + - void accept_Tptr "accept_Tptr"(shared_ptr[T]& value) except + - pair[T,shared_ptr[T]] create_MixedPtrs "create_MixedPtrs"() except + - pair[shared_ptr[T],shared_ptr[T]] create_ptrs "create_ptrs"() except + - T return_T "return_T"(shared_ptr[T]& value) except + - shared_ptr[T] return_Tptr "return_Tptr"(shared_ptr[T]& value) except + - pair[shared_ptr[T],shared_ptr[T]] return_ptrs "return_ptrs"(shared_ptr[T]& p1, shared_ptr[T]& p2) except + + void accept_T(const T& value) except + + void accept_Tptr(shared_ptr[T]& value) except + + pair[T,shared_ptr[T]] create_MixedPtrs() except + + pair[shared_ptr[T],shared_ptr[T]] create_ptrs() except + + T return_T(shared_ptr[T]& value) except + + shared_ptr[T] return_Tptr(shared_ptr[T]& value) except + + pair[shared_ptr[T],shared_ptr[T]] return_ptrs(shared_ptr[T]& p1, shared_ptr[T]& p2) except + ARG templatedMethod[ARG](const ARG& t) except + + ctypedef CMyTemplate[CPoint2] CMyTemplatePoint2 cdef class MyTemplatePoint2(MyBase): - cdef shared_ptr[CMyTemplatePoint2] shared_CMyTemplatePoint2_ + cdef shared_ptr[CMyTemplatePoint2] CMyTemplatePoint2_ @staticmethod cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other) + ctypedef CMyTemplate[MatrixXd] CMyTemplateMatrix cdef class MyTemplateMatrix(MyBase): - cdef shared_ptr[CMyTemplateMatrix] shared_CMyTemplateMatrix_ + cdef shared_ptr[CMyTemplateMatrix] CMyTemplateMatrix_ @staticmethod cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other) @@ -124,10 +129,11 @@ cdef extern from "folder/path/to/Test.h": cdef cppclass CMyFactor "MyFactor"[POSE,POINT]: CMyFactor(size_t key1, size_t key2, double measured, const shared_ptr[CnoiseModel_Base]& noiseModel) except + + ctypedef CMyFactor[CPose2, MatrixXd] CMyFactorPosePoint2 cdef class MyFactorPosePoint2: - cdef shared_ptr[CMyFactorPosePoint2] shared_CMyFactorPosePoint2_ + cdef shared_ptr[CMyFactorPosePoint2] CMyFactorPosePoint2_ @staticmethod cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other) diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index f7361500d..0a4e24edf 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -4,7 +4,17 @@ cimport geometry from geometry cimport shared_ptr from geometry cimport dynamic_pointer_cast from geometry cimport make_shared -from eigency.core cimport * +# C helper function that copies all arguments into a positional list. +cdef list process_args(list keywords, tuple args, dict kwargs): + cdef str keyword + cdef int n = len(args), m = len(keywords) + cdef list params = list(args) + assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m) + try: + return params + [kwargs[keyword] for keyword in keywords[n:]] + except: + raise ValueError('Epected arguments ' + str(keywords)) +from gtsam_eigency.core cimport * from libcpp cimport bool from libcpp.pair cimport pair @@ -14,104 +24,83 @@ from cython.operator cimport dereference as deref cdef class Point2: def __init__(self, *args, **kwargs): - self.shared_CPoint2_ = shared_ptr[CPoint2]() + cdef list __params + self.CPoint2_ = shared_ptr[CPoint2]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.Point2_0(*args, **kwargs): - pass - elif self.Point2_1(*args, **kwargs): - pass - else: - raise TypeError('Point2 construction failed!') - - def Point2_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=0: - return False - self.shared_CPoint2_ = shared_ptr[CPoint2](new CPoint2()) - return True - - def Point2_1(self, *args, **kwargs): - if len(args)+len(kwargs) !=2: - return False - __params = kwargs.copy() - __names = ['x', 'y'] - for i in range(len(args)): - __params[__names[i]] = args[i] try: - x = (__params['x']) - y = (__params['y']) + __params = process_args([], args, kwargs) + self.CPoint2_ = shared_ptr[CPoint2](new CPoint2()) except: - return False - self.shared_CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y)) - return True - + pass + try: + __params = process_args(['x', 'y'], args, kwargs) + x = (__params[0]) + y = (__params[1]) + self.CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y)) + except: + pass + if (self.CPoint2_.use_count()==0): + raise TypeError('Point2 construction failed!') @staticmethod cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef Point2 ret = Point2(cyCreateFromShared=True) - ret.shared_CPoint2_ = other - return ret + cdef Point2 return_value = Point2(cyCreateFromShared=True) + return_value.CPoint2_ = other + return return_value + def argChar(self, char a): - self.shared_CPoint2_.get().argChar(a) + self.CPoint2_.get().argChar(a) def argUChar(self, unsigned char a): - self.shared_CPoint2_.get().argUChar(a) + self.CPoint2_.get().argUChar(a) def dim(self): - cdef int ret = self.shared_CPoint2_.get().dim() + cdef int ret = self.CPoint2_.get().dim() return ret def eigenArguments(self, np.ndarray v, np.ndarray m): v = v.astype(float, order='F', copy=False) m = m.astype(float, order='F', copy=False) - self.shared_CPoint2_.get().eigenArguments((Map[VectorXd](v)), (Map[MatrixXd](m))) + self.CPoint2_.get().eigenArguments((Map[VectorXd](v)), (Map[MatrixXd](m))) def returnChar(self): - cdef char ret = self.shared_CPoint2_.get().returnChar() + cdef char ret = self.CPoint2_.get().returnChar() return ret def vectorConfusion(self): - cdef shared_ptr[CVectorNotEigen] ret = make_shared[CVectorNotEigen](self.shared_CPoint2_.get().vectorConfusion()) + cdef shared_ptr[CVectorNotEigen] ret = make_shared[CVectorNotEigen](self.CPoint2_.get().vectorConfusion()) return VectorNotEigen.cyCreateFromShared(ret) def x(self): - cdef double ret = self.shared_CPoint2_.get().x() + cdef double ret = self.CPoint2_.get().x() return ret def y(self): - cdef double ret = self.shared_CPoint2_.get().y() + cdef double ret = self.CPoint2_.get().y() return ret cdef class Point3: def __init__(self, *args, **kwargs): - self.shared_CPoint3_ = shared_ptr[CPoint3]() + cdef list __params + self.CPoint3_ = shared_ptr[CPoint3]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.Point3_0(*args, **kwargs): - pass - else: - raise TypeError('Point3 construction failed!') - - def Point3_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=3: - return False - __params = kwargs.copy() - __names = ['x', 'y', 'z'] - for i in range(len(args)): - __params[__names[i]] = args[i] try: - x = (__params['x']) - y = (__params['y']) - z = (__params['z']) + __params = process_args(['x', 'y', 'z'], args, kwargs) + x = (__params[0]) + y = (__params[1]) + z = (__params[2]) + self.CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z)) except: - return False - self.shared_CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z)) - return True - + pass + if (self.CPoint3_.use_count()==0): + raise TypeError('Point3 construction failed!') @staticmethod cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef Point3 ret = Point3(cyCreateFromShared=True) - ret.shared_CPoint3_ = other - return ret + cdef Point3 return_value = Point3(cyCreateFromShared=True) + return_value.CPoint3_ = other + return return_value + @staticmethod def StaticFunctionRet(double z): return Point3.cyCreateFromShared(make_shared[CPoint3](CPoint3.StaticFunctionRet(z))) @@ -122,365 +111,337 @@ cdef class Point3: def norm(self): - cdef double ret = self.shared_CPoint3_.get().norm() + cdef double ret = self.CPoint3_.get().norm() return ret cdef class Test: def __init__(self, *args, **kwargs): - self.shared_CTest_ = shared_ptr[CTest]() + cdef list __params + self.CTest_ = shared_ptr[CTest]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.Test_0(*args, **kwargs): - pass - elif self.Test_1(*args, **kwargs): - pass - else: - raise TypeError('Test construction failed!') - - def Test_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=0: - return False - self.shared_CTest_ = shared_ptr[CTest](new CTest()) - return True - - def Test_1(self, *args, **kwargs): - if len(args)+len(kwargs) !=2: - return False - __params = kwargs.copy() - __names = ['a', 'b'] - for i in range(len(args)): - __params[__names[i]] = args[i] - if not isinstance(__params[__names[1]], np.ndarray) or not __params[__names[1]].ndim == 2: - return False try: - a = (__params['a']) - b = (__params['b']) + __params = process_args([], args, kwargs) + self.CTest_ = shared_ptr[CTest](new CTest()) except: - return False - b = b.astype(float, order='F', copy=False) - self.shared_CTest_ = shared_ptr[CTest](new CTest(a, (Map[MatrixXd](b)))) - return True - + pass + try: + __params = process_args(['a', 'b'], args, kwargs) + a = (__params[0]) + b = (__params[1]) + assert isinstance(b, np.ndarray) and b.ndim == 2 + b = b.astype(float, order='F', copy=False) + self.CTest_ = shared_ptr[CTest](new CTest(a, (Map[MatrixXd](b)))) + except: + pass + if (self.CTest_.use_count()==0): + raise TypeError('Test construction failed!') @staticmethod cdef Test cyCreateFromShared(const shared_ptr[CTest]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef Test ret = Test(cyCreateFromShared=True) - ret.shared_CTest_ = other - return ret + cdef Test return_value = Test(cyCreateFromShared=True) + return_value.CTest_ = other + return return_value + def arg_EigenConstRef(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - self.shared_CTest_.get().arg_EigenConstRef((Map[MatrixXd](value))) + self.CTest_.get().arg_EigenConstRef((Map[MatrixXd](value))) def create_MixedPtrs(self): - cdef pair [CTest,shared_ptr[CTest]] ret = self.shared_CTest_.get().create_MixedPtrs() + cdef pair [CTest,shared_ptr[CTest]] ret = self.CTest_.get().create_MixedPtrs() return (Test.cyCreateFromShared(make_shared[CTest](ret.first)),Test.cyCreateFromShared(ret.second)) def create_ptrs(self): - cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.shared_CTest_.get().create_ptrs() + cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().create_ptrs() return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second)) def __str__(self): + strBuf = RedirectCout() self.print_('') - return '' + return strBuf.str() def print_(self): - self.shared_CTest_.get().print_() + self.CTest_.get().print_() def return_Point2Ptr(self, bool value): - cdef shared_ptr[CPoint2] ret = self.shared_CTest_.get().return_Point2Ptr(value) + cdef shared_ptr[CPoint2] ret = self.CTest_.get().return_Point2Ptr(value) return Point2.cyCreateFromShared(ret) def return_Test(self, Test value): - cdef shared_ptr[CTest] ret = make_shared[CTest](self.shared_CTest_.get().return_Test(value.shared_CTest_)) + cdef shared_ptr[CTest] ret = make_shared[CTest](self.CTest_.get().return_Test(value.CTest_)) return Test.cyCreateFromShared(ret) def return_TestPtr(self, Test value): - cdef shared_ptr[CTest] ret = self.shared_CTest_.get().return_TestPtr(value.shared_CTest_) + cdef shared_ptr[CTest] ret = self.CTest_.get().return_TestPtr(value.CTest_) return Test.cyCreateFromShared(ret) def return_bool(self, bool value): - cdef bool ret = self.shared_CTest_.get().return_bool(value) + cdef bool ret = self.CTest_.get().return_bool(value) return ret def return_double(self, double value): - cdef double ret = self.shared_CTest_.get().return_double(value) + cdef double ret = self.CTest_.get().return_double(value) return ret def return_field(self, Test t): - cdef bool ret = self.shared_CTest_.get().return_field(deref(t.shared_CTest_)) + cdef bool ret = self.CTest_.get().return_field(deref(t.CTest_)) return ret def return_int(self, int value): - cdef int ret = self.shared_CTest_.get().return_int(value) + cdef int ret = self.CTest_.get().return_int(value) return ret def return_matrix1(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - cdef MatrixXd ret = self.shared_CTest_.get().return_matrix1((Map[MatrixXd](value))) + cdef MatrixXd ret = self.CTest_.get().return_matrix1((Map[MatrixXd](value))) return ndarray_copy(ret) def return_matrix2(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - cdef MatrixXd ret = self.shared_CTest_.get().return_matrix2((Map[MatrixXd](value))) + cdef MatrixXd ret = self.CTest_.get().return_matrix2((Map[MatrixXd](value))) return ndarray_copy(ret) def return_pair(self, np.ndarray v, np.ndarray A): v = v.astype(float, order='F', copy=False) A = A.astype(float, order='F', copy=False) - cdef pair [VectorXd,MatrixXd] ret = self.shared_CTest_.get().return_pair((Map[VectorXd](v)), (Map[MatrixXd](A))) + cdef pair [VectorXd,MatrixXd] ret = self.CTest_.get().return_pair((Map[VectorXd](v)), (Map[MatrixXd](A))) return (ndarray_copy(ret.first).squeeze(),ndarray_copy(ret.second)) def return_ptrs(self, Test p1, Test p2): - cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.shared_CTest_.get().return_ptrs(p1.shared_CTest_, p2.shared_CTest_) + cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().return_ptrs(p1.CTest_, p2.CTest_) return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second)) def return_size_t(self, size_t value): - cdef size_t ret = self.shared_CTest_.get().return_size_t(value) + cdef size_t ret = self.CTest_.get().return_size_t(value) return ret def return_string(self, string value): - cdef string ret = self.shared_CTest_.get().return_string(value) + cdef string ret = self.CTest_.get().return_string(value) return ret def return_vector1(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - cdef VectorXd ret = self.shared_CTest_.get().return_vector1((Map[VectorXd](value))) + cdef VectorXd ret = self.CTest_.get().return_vector1((Map[VectorXd](value))) return ndarray_copy(ret).squeeze() def return_vector2(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - cdef VectorXd ret = self.shared_CTest_.get().return_vector2((Map[VectorXd](value))) + cdef VectorXd ret = self.CTest_.get().return_vector2((Map[VectorXd](value))) return ndarray_copy(ret).squeeze() cdef class MyBase: def __init__(self, *args, **kwargs): - self.shared_CMyBase_ = shared_ptr[CMyBase]() + cdef list __params + self.CMyBase_ = shared_ptr[CMyBase]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - else: + if (self.CMyBase_.use_count()==0): raise TypeError('MyBase construction failed!') @staticmethod cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef MyBase ret = MyBase(cyCreateFromShared=True) - ret.shared_CMyBase_ = other - return ret + cdef MyBase return_value = MyBase(cyCreateFromShared=True) + return_value.CMyBase_ = other + return return_value + cdef class MyTemplatePoint2(MyBase): def __init__(self, *args, **kwargs): - self.shared_CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2]() + cdef list __params + self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.MyTemplatePoint2_0(*args, **kwargs): + try: + __params = process_args([], args, kwargs) + self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2()) + except: pass - else: + if (self.CMyTemplatePoint2_.use_count()==0): raise TypeError('MyTemplatePoint2 construction failed!') - self.shared_CMyBase_ = (self.shared_CMyTemplatePoint2_) - - def MyTemplatePoint2_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=0: - return False - self.shared_CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2()) - return True - + self.CMyBase_ = (self.CMyTemplatePoint2_) @staticmethod cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef MyTemplatePoint2 ret = MyTemplatePoint2(cyCreateFromShared=True) - ret.shared_CMyTemplatePoint2_ = other - ret.shared_CMyBase_ = (other) - return ret + cdef MyTemplatePoint2 return_value = MyTemplatePoint2(cyCreateFromShared=True) + return_value.CMyTemplatePoint2_ = other + return_value.CMyBase_ = (other) + return return_value + def accept_T(self, Point2 value): - self.shared_CMyTemplatePoint2_.get().accept_T(deref(value.shared_CPoint2_)) + self.CMyTemplatePoint2_.get().accept_T(deref(value.CPoint2_)) def accept_Tptr(self, Point2 value): - self.shared_CMyTemplatePoint2_.get().accept_Tptr(value.shared_CPoint2_) + self.CMyTemplatePoint2_.get().accept_Tptr(value.CPoint2_) def create_MixedPtrs(self): - cdef pair [CPoint2,shared_ptr[CPoint2]] ret = self.shared_CMyTemplatePoint2_.get().create_MixedPtrs() + cdef pair [CPoint2,shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_MixedPtrs() return (Point2.cyCreateFromShared(make_shared[CPoint2](ret.first)),Point2.cyCreateFromShared(ret.second)) def create_ptrs(self): - cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.shared_CMyTemplatePoint2_.get().create_ptrs() + cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_ptrs() return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second)) def return_T(self, Point2 value): - cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.shared_CMyTemplatePoint2_.get().return_T(value.shared_CPoint2_)) + cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().return_T(value.CPoint2_)) return Point2.cyCreateFromShared(ret) def return_Tptr(self, Point2 value): - cdef shared_ptr[CPoint2] ret = self.shared_CMyTemplatePoint2_.get().return_Tptr(value.shared_CPoint2_) + cdef shared_ptr[CPoint2] ret = self.CMyTemplatePoint2_.get().return_Tptr(value.CPoint2_) return Point2.cyCreateFromShared(ret) def return_ptrs(self, Point2 p1, Point2 p2): - cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.shared_CMyTemplatePoint2_.get().return_ptrs(p1.shared_CPoint2_, p2.shared_CPoint2_) + cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().return_ptrs(p1.CPoint2_, p2.CPoint2_) return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second)) def templatedMethodMatrix(self, np.ndarray t): t = t.astype(float, order='F', copy=False) - cdef MatrixXd ret = self.shared_CMyTemplatePoint2_.get().templatedMethod[MatrixXd]((Map[MatrixXd](t))) + cdef MatrixXd ret = self.CMyTemplatePoint2_.get().templatedMethod[MatrixXd]((Map[MatrixXd](t))) return ndarray_copy(ret) def templatedMethodPoint2(self, Point2 t): - cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.shared_CMyTemplatePoint2_.get().templatedMethod[CPoint2](deref(t.shared_CPoint2_))) + cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().templatedMethod[CPoint2](deref(t.CPoint2_))) return Point2.cyCreateFromShared(ret) def templatedMethodPoint3(self, Point3 t): - cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.shared_CMyTemplatePoint2_.get().templatedMethod[CPoint3](deref(t.shared_CPoint3_))) + cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplatePoint2_.get().templatedMethod[CPoint3](deref(t.CPoint3_))) return Point3.cyCreateFromShared(ret) def templatedMethodVector(self, np.ndarray t): t = t.astype(float, order='F', copy=False) - cdef VectorXd ret = self.shared_CMyTemplatePoint2_.get().templatedMethod[VectorXd]((Map[VectorXd](t))) + cdef VectorXd ret = self.CMyTemplatePoint2_.get().templatedMethod[VectorXd]((Map[VectorXd](t))) return ndarray_copy(ret).squeeze() def dynamic_cast_MyTemplatePoint2_MyBase(MyBase parent): try: - return MyTemplatePoint2.cyCreateFromShared(dynamic_pointer_cast[CMyTemplatePoint2,CMyBase](parent.shared_CMyBase_)) + return MyTemplatePoint2.cyCreateFromShared(dynamic_pointer_cast[CMyTemplatePoint2,CMyBase](parent.CMyBase_)) except: raise TypeError('dynamic cast failed!') cdef class MyTemplateMatrix(MyBase): def __init__(self, *args, **kwargs): - self.shared_CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix]() + cdef list __params + self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.MyTemplateMatrix_0(*args, **kwargs): + try: + __params = process_args([], args, kwargs) + self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix()) + except: pass - else: + if (self.CMyTemplateMatrix_.use_count()==0): raise TypeError('MyTemplateMatrix construction failed!') - self.shared_CMyBase_ = (self.shared_CMyTemplateMatrix_) - - def MyTemplateMatrix_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=0: - return False - self.shared_CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix()) - return True - + self.CMyBase_ = (self.CMyTemplateMatrix_) @staticmethod cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef MyTemplateMatrix ret = MyTemplateMatrix(cyCreateFromShared=True) - ret.shared_CMyTemplateMatrix_ = other - ret.shared_CMyBase_ = (other) - return ret + cdef MyTemplateMatrix return_value = MyTemplateMatrix(cyCreateFromShared=True) + return_value.CMyTemplateMatrix_ = other + return_value.CMyBase_ = (other) + return return_value + def accept_T(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - self.shared_CMyTemplateMatrix_.get().accept_T((Map[MatrixXd](value))) + self.CMyTemplateMatrix_.get().accept_T((Map[MatrixXd](value))) def accept_Tptr(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - self.shared_CMyTemplateMatrix_.get().accept_Tptr((Map[MatrixXd](value))) + self.CMyTemplateMatrix_.get().accept_Tptr((Map[MatrixXd](value))) def create_MixedPtrs(self): - cdef pair [MatrixXd,shared_ptr[MatrixXd]] ret = self.shared_CMyTemplateMatrix_.get().create_MixedPtrs() + cdef pair [MatrixXd,shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_MixedPtrs() return (ndarray_copy(ret.first),ndarray_copy(ret.second)) def create_ptrs(self): - cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.shared_CMyTemplateMatrix_.get().create_ptrs() + cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_ptrs() return (ndarray_copy(ret.first),ndarray_copy(ret.second)) def return_T(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - cdef MatrixXd ret = self.shared_CMyTemplateMatrix_.get().return_T((Map[MatrixXd](value))) + cdef MatrixXd ret = self.CMyTemplateMatrix_.get().return_T((Map[MatrixXd](value))) return ndarray_copy(ret) def return_Tptr(self, np.ndarray value): value = value.astype(float, order='F', copy=False) - cdef shared_ptr[MatrixXd] ret = self.shared_CMyTemplateMatrix_.get().return_Tptr((Map[MatrixXd](value))) + cdef shared_ptr[MatrixXd] ret = self.CMyTemplateMatrix_.get().return_Tptr((Map[MatrixXd](value))) return ndarray_copy(ret) def return_ptrs(self, np.ndarray p1, np.ndarray p2): p1 = p1.astype(float, order='F', copy=False) p2 = p2.astype(float, order='F', copy=False) - cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.shared_CMyTemplateMatrix_.get().return_ptrs((Map[MatrixXd](p1)), (Map[MatrixXd](p2))) + cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().return_ptrs((Map[MatrixXd](p1)), (Map[MatrixXd](p2))) return (ndarray_copy(ret.first),ndarray_copy(ret.second)) def templatedMethodMatrix(self, np.ndarray t): t = t.astype(float, order='F', copy=False) - cdef MatrixXd ret = self.shared_CMyTemplateMatrix_.get().templatedMethod[MatrixXd]((Map[MatrixXd](t))) + cdef MatrixXd ret = self.CMyTemplateMatrix_.get().templatedMethod[MatrixXd]((Map[MatrixXd](t))) return ndarray_copy(ret) def templatedMethodPoint2(self, Point2 t): - cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.shared_CMyTemplateMatrix_.get().templatedMethod[CPoint2](deref(t.shared_CPoint2_))) + cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplateMatrix_.get().templatedMethod[CPoint2](deref(t.CPoint2_))) return Point2.cyCreateFromShared(ret) def templatedMethodPoint3(self, Point3 t): - cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.shared_CMyTemplateMatrix_.get().templatedMethod[CPoint3](deref(t.shared_CPoint3_))) + cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplateMatrix_.get().templatedMethod[CPoint3](deref(t.CPoint3_))) return Point3.cyCreateFromShared(ret) def templatedMethodVector(self, np.ndarray t): t = t.astype(float, order='F', copy=False) - cdef VectorXd ret = self.shared_CMyTemplateMatrix_.get().templatedMethod[VectorXd]((Map[VectorXd](t))) + cdef VectorXd ret = self.CMyTemplateMatrix_.get().templatedMethod[VectorXd]((Map[VectorXd](t))) return ndarray_copy(ret).squeeze() def dynamic_cast_MyTemplateMatrix_MyBase(MyBase parent): try: - return MyTemplateMatrix.cyCreateFromShared(dynamic_pointer_cast[CMyTemplateMatrix,CMyBase](parent.shared_CMyBase_)) + return MyTemplateMatrix.cyCreateFromShared(dynamic_pointer_cast[CMyTemplateMatrix,CMyBase](parent.CMyBase_)) except: raise TypeError('dynamic cast failed!') cdef class MyVector3: def __init__(self, *args, **kwargs): - self.shared_CMyVector3_ = shared_ptr[CMyVector3]() + cdef list __params + self.CMyVector3_ = shared_ptr[CMyVector3]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.MyVector3_0(*args, **kwargs): + try: + __params = process_args([], args, kwargs) + self.CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3()) + except: pass - else: + if (self.CMyVector3_.use_count()==0): raise TypeError('MyVector3 construction failed!') - def MyVector3_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=0: - return False - self.shared_CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3()) - return True - - @staticmethod cdef MyVector3 cyCreateFromShared(const shared_ptr[CMyVector3]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef MyVector3 ret = MyVector3(cyCreateFromShared=True) - ret.shared_CMyVector3_ = other - return ret + cdef MyVector3 return_value = MyVector3(cyCreateFromShared=True) + return_value.CMyVector3_ = other + return return_value + cdef class MyVector12: def __init__(self, *args, **kwargs): - self.shared_CMyVector12_ = shared_ptr[CMyVector12]() + cdef list __params + self.CMyVector12_ = shared_ptr[CMyVector12]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.MyVector12_0(*args, **kwargs): + try: + __params = process_args([], args, kwargs) + self.CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12()) + except: pass - else: + if (self.CMyVector12_.use_count()==0): raise TypeError('MyVector12 construction failed!') - def MyVector12_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=0: - return False - self.shared_CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12()) - return True - - @staticmethod cdef MyVector12 cyCreateFromShared(const shared_ptr[CMyVector12]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef MyVector12 ret = MyVector12(cyCreateFromShared=True) - ret.shared_CMyVector12_ = other - return ret + cdef MyVector12 return_value = MyVector12(cyCreateFromShared=True) + return_value.CMyVector12_ = other + return return_value + cdef class MyFactorPosePoint2: def __init__(self, *args, **kwargs): - self.shared_CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2]() + cdef list __params + self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2]() if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'): return - elif self.MyFactorPosePoint2_0(*args, **kwargs): - pass - else: - raise TypeError('MyFactorPosePoint2 construction failed!') - - def MyFactorPosePoint2_0(self, *args, **kwargs): - if len(args)+len(kwargs) !=4: - return False - __params = kwargs.copy() - __names = ['key1', 'key2', 'measured', 'noiseModel'] - for i in range(len(args)): - __params[__names[i]] = args[i] - if not isinstance(__params[__names[3]], noiseModel_Base): - return False try: - key1 = (__params['key1']) - key2 = (__params['key2']) - measured = (__params['measured']) - noiseModel = (__params['noiseModel']) + __params = process_args(['key1', 'key2', 'measured', 'noiseModel'], args, kwargs) + key1 = (__params[0]) + key2 = (__params[1]) + measured = (__params[2]) + noiseModel = (__params[3]) + assert isinstance(noiseModel, noiseModel_Base) + self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.CnoiseModel_Base_)) except: - return False - self.shared_CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.shared_CnoiseModel_Base_)) - return True - + pass + if (self.CMyFactorPosePoint2_.use_count()==0): + raise TypeError('MyFactorPosePoint2 construction failed!') @staticmethod cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other): if other.get() == NULL: raise RuntimeError('Cannot create object from a nullptr!') - cdef MyFactorPosePoint2 ret = MyFactorPosePoint2(cyCreateFromShared=True) - ret.shared_CMyFactorPosePoint2_ = other - return ret + cdef MyFactorPosePoint2 return_value = MyFactorPosePoint2(cyCreateFromShared=True) + return_value.CMyFactorPosePoint2_ = other + return return_value + @@ -488,37 +449,33 @@ def aGlobalFunction(): cdef VectorXd ret = pxd_aGlobalFunction() return ndarray_copy(ret).squeeze() def overloadedGlobalFunction(*args, **kwargs): - success, results = overloadedGlobalFunction_0(*args, **kwargs) + success, results = overloadedGlobalFunction_0(args, kwargs) if success: return results - success, results = overloadedGlobalFunction_1(*args, **kwargs) + success, results = overloadedGlobalFunction_1(args, kwargs) if success: return results raise TypeError('Could not find the correct overload') -def overloadedGlobalFunction_0(*args, **kwargs): - if len(args)+len(kwargs) !=1: - return False, None - __params = kwargs.copy() - __names = ['a'] - for i in range(len(args)): - __params[__names[i]] = args[i] +def overloadedGlobalFunction_0(args, kwargs): + cdef list __params + cdef VectorXd return_value try: - a = (__params['a']) + __params = process_args(['a'], args, kwargs) + a = (__params[0]) + return_value = pxd_overloadedGlobalFunction(a) + return True, ndarray_copy(return_value).squeeze() except: return False, None - cdef VectorXd ret = pxd_overloadedGlobalFunction(a) - return True, ndarray_copy(ret).squeeze() -def overloadedGlobalFunction_1(*args, **kwargs): - if len(args)+len(kwargs) !=2: - return False, None - __params = kwargs.copy() - __names = ['a', 'b'] - for i in range(len(args)): - __params[__names[i]] = args[i] + +def overloadedGlobalFunction_1(args, kwargs): + cdef list __params + cdef VectorXd return_value try: - a = (__params['a']) - b = (__params['b']) + __params = process_args(['a', 'b'], args, kwargs) + a = (__params[0]) + b = (__params[1]) + return_value = pxd_overloadedGlobalFunction(a, b) + return True, ndarray_copy(return_value).squeeze() except: return False, None - cdef VectorXd ret = pxd_overloadedGlobalFunction(a, b) - return True, ndarray_copy(ret).squeeze() +