diff --git a/CMakeLists.txt b/CMakeLists.txt index 0380b8a2f..e8da98ffd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,7 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -344,6 +345,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) endif() +# Python wrap +if (GTSAM_BUILD_PYTHON) + include(GtsamPythonWrap) + + # NOTE: The automatic generation of python wrapper from the gtsampy.h interface is + # not working yet, so we're using a handwritten wrapper files on python/handwritten. + # Once the python wrapping from the interface file is working, you can _swap_ the + # comments on the next lines + + # wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "") + add_subdirectory(python) + +endif() + # Build gtsam_unstable if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) @@ -460,6 +475,17 @@ print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full Ex message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") + +message(STATUS "Python module flags ") + +if(GTSAM_PYTHON_WARNINGS) + message(STATUS " Build python module : No - dependencies missing") +else() + print_config_flag(${GTSAM_BUILD_PYTHON} "Build python module ") +endif() +if(GTSAM_BUILD_PYTHON) + message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") +endif() message(STATUS "===============================================================") # Print warnings at the end @@ -472,6 +498,9 @@ endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") endif() +if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) + message(WARNING "${GTSAM_PYTHON_WARNINGS}") +endif() # Include CPack *after* all flags include(CPack) diff --git a/THANKS b/THANKS index 9c06a5d28..f84cfa185 100644 --- a/THANKS +++ b/THANKS @@ -38,6 +38,10 @@ at Uni Zurich: * Christian Forster +at LAAS-CNRS + +* Ellon Paiva + Many thanks for your hard work!!!! Frank Dellaert diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake new file mode 100644 index 000000000..eafed165e --- /dev/null +++ b/cmake/FindNumPy.cmake @@ -0,0 +1,102 @@ +# - Find the NumPy libraries +# This module finds if NumPy is installed, and sets the following variables +# indicating where it is. +# +# TODO: Update to provide the libraries and paths for linking npymath lib. +# +# NUMPY_FOUND - was NumPy found +# NUMPY_VERSION - the version of NumPy found as a string +# NUMPY_VERSION_MAJOR - the major version number of NumPy +# NUMPY_VERSION_MINOR - the minor version number of NumPy +# NUMPY_VERSION_PATCH - the patch version number of NumPy +# NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601 +# NUMPY_INCLUDE_DIRS - path to the NumPy include files + +#============================================================================ +# Copyright 2012 Continuum Analytics, Inc. +# +# MIT License +# +# 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. +# +#============================================================================ + +# Finding NumPy involves calling the Python interpreter +if(NumPy_FIND_REQUIRED) + find_package(PythonInterp REQUIRED) +else() + find_package(PythonInterp) +endif() + +if(NOT PYTHONINTERP_FOUND) + set(NUMPY_FOUND FALSE) + return() +endif() + +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "import numpy as n; print(n.__version__); print(n.get_include());" + RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS + OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT + ERROR_VARIABLE _NUMPY_ERROR_VALUE + OUTPUT_STRIP_TRAILING_WHITESPACE) + +if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0) + if(NumPy_FIND_REQUIRED) + message(FATAL_ERROR + "NumPy import failure:\n${_NUMPY_ERROR_VALUE}") + endif() + set(NUMPY_FOUND FALSE) + return() +endif() + +# Convert the process output into a list +string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT}) +string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES}) +# Just in case there is unexpected output from the Python command. +list(GET _NUMPY_VALUES -2 NUMPY_VERSION) +list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS) + +string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}") +if("${_VER_CHECK}" STREQUAL "") + # The output from Python was unexpected. Raise an error always + # here, because we found NumPy, but it appears to be corrupted somehow. + message(FATAL_ERROR + "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n") + return() +endif() + +# Make sure all directory separators are '/' +string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS}) + +# Get the major and minor version numbers +string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION}) +list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR) +list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR) +list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH) +string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH}) +math(EXPR NUMPY_VERSION_DECIMAL + "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}") + +find_package_message(NUMPY + "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}" + "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}") + +set(NUMPY_FOUND TRUE) + diff --git a/cmake/GtsamPythonWrap.cmake b/cmake/GtsamPythonWrap.cmake index d065a7828..714e37488 100644 --- a/cmake/GtsamPythonWrap.cmake +++ b/cmake/GtsamPythonWrap.cmake @@ -1,5 +1,5 @@ #Setup cache options -option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version") set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation") set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python") if(NOT GTSAM_PYTHON_INSTALL_PATH) @@ -8,13 +8,13 @@ endif() #Author: Paul Furgale Modified by Andrew Melim function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) - # Boost - find_package(Boost COMPONENTS python filesystem system REQUIRED) - include_directories(${Boost_INCLUDE_DIRS}) + # # Boost + # find_package(Boost COMPONENTS python filesystem system REQUIRED) + # include_directories(${Boost_INCLUDE_DIRS}) - # Find Python - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) - INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) + # # Find Python + # FIND_PACKAGE(PythonLibs 2.7 REQUIRED) + # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) IF(APPLE) # The apple framework headers don't include the numpy headers for some reason. @@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) ENDIF() ENDIF(APPLE) + if(MSVC) + add_library(${moduleName}_python MODULE ${ARGN}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1 + VERSION 1 + SOVERSION 0 + SUFFIX ".pyd") + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp - # Create a static library version - add_library(${TARGET_NAME} SHARED ${ARGN}) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd) - target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared) - set_target_properties(${TARGET_NAME} PROPERTIES - OUTPUT_NAME ${TARGET_NAME} - CLEAN_DIRECT_OUTPUT 1 - VERSION 1 - SOVERSION 0) + ELSE() + # Create a shared library + add_library(${moduleName}_python SHARED ${generated_cpp_file}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1) + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp + # On OSX and Linux, the python library must end in the extension .so. Build this + # filename here. + get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME lib${moduleName}_python.so) + ENDIF(MSVC) - # On OSX and Linux, the python library must end in the extension .so. Build this - # filename here. - get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION) - get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) - set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so) + # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package + set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam) + # Cause the library to be output in the correct directory. + add_custom_command(TARGET ${moduleName}_python + POST_BUILD + COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + COMMENT "Copying library files to python directory" ) # Cause the library to be output in the correct directory. add_custom_command(TARGET ${TARGET_NAME} @@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES) list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}) set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") -endfunction(wrap_python) \ No newline at end of file +endfunction(wrap_python) + +# Macro to get list of subdirectories +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() diff --git a/gtsam/3rdparty/numpy_eigen/README.md b/gtsam/3rdparty/numpy_eigen/README.md new file mode 100644 index 000000000..bd5a3f44d --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/README.md @@ -0,0 +1,6 @@ +numpy_eigen +=========== + +A boost python converter that handles the copy of matrices from the Eigen linear algebra library in C++ to numpy in Python. + +This is a minimal version based on the original from Paul Furgale (https://github.com/ethz-asl/Schweizer-Messer/tree/master/numpy_eigen) \ No newline at end of file diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp new file mode 100755 index 000000000..4cf16a974 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/NumpyEigenConverter.hpp @@ -0,0 +1,334 @@ +/** + * @file NumpyEigenConverter.hpp + * @author Paul Furgale + * @date Fri Feb 4 11:17:25 2011 + * + * @brief Classes to support conversion from numpy arrays in Python + * to Eigen3 matrices in c++ + * + * + */ + +#ifndef NUMPY_EIGEN_CONVERTER_HPP +#define NUMPY_EIGEN_CONVERTER_HPP + +#include +//#include + +#include "numpy/numpyconfig.h" +#ifdef NPY_1_7_API_VERSION +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#define NPE_PY_ARRAY_OBJECT PyArrayObject +#else +//TODO Remove this as soon as support for Numpy version before 1.7 is dropped +#define NPE_PY_ARRAY_OBJECT PyObject +#endif + +#define PY_ARRAY_UNIQUE_SYMBOL NP_Eigen_AS +#include + +#include "type_traits.hpp" +#include +#include "copy_routines.hpp" + + + +/** + * @class NumpyEigenConverter + * @tparam the Eigen3 matrix type this class is specialized for + * + * adapted from http://misspent.wordpress.com/2009/09/27/how-to-write-boost-python-converters/ + * General help available http://docs.scipy.org/doc/numpy/reference/c-api.array.html + * + * To use: + * + * #include + * + * + * BOOST_PYTHON_MODULE(libmy_module_python) + * { + * // The converters will cause a segfault unless import_array() is called before the first one + * import_array(); + * NumpyEigenConverter >::register_converter(); + * NumpyEigenConverter >::register_converter(); + * } + * + */ +template +struct NumpyEigenConverter +{ + + typedef EIGEN_MATRIX_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + enum { + RowsAtCompileTime = matrix_t::RowsAtCompileTime, + ColsAtCompileTime = matrix_t::ColsAtCompileTime, + MaxRowsAtCompileTime = matrix_t::MaxRowsAtCompileTime, + MaxColsAtCompileTime = matrix_t::MaxColsAtCompileTime, + NpyType = TypeToNumPy::NpyType, + //Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, + //CoeffReadCost = NumTraits::ReadCost, + Options = matrix_t::Options + //InnerStrideAtCompileTime = 1, + //OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime + }; + + static std::string castSizeOption(int option) + { + if(option == Eigen::Dynamic) + return "Dynamic"; + else + return boost::lexical_cast(option); + } + + static std::string toString() + { + return std::string() + "Eigen::Matrix<" + TypeToNumPy::typeString() + ", " + + castSizeOption(RowsAtCompileTime) + ", " + + castSizeOption(ColsAtCompileTime) + ", " + + boost::lexical_cast((int)Options) + ", " + + castSizeOption(MaxRowsAtCompileTime) + ", " + + castSizeOption(MaxColsAtCompileTime) + ">"; + } + + // The "Convert from C to Python" API + static PyObject * convert(const matrix_t & M) + { + PyObject * P = NULL; + if(RowsAtCompileTime == 1 || ColsAtCompileTime == 1) + { + // Create a 1D array + npy_intp dimensions[1]; + dimensions[0] = M.size(); + P = PyArray_SimpleNew(1, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyVector >(&M, reinterpret_cast(P)); + } + else + { + // create a 2D array. + npy_intp dimensions[2]; + dimensions[0] = M.rows(); + dimensions[1] = M.cols(); + P = PyArray_SimpleNew(2, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyMatrix >(&M, reinterpret_cast(P)); + } + + // incrementing the reference seems to cause a memory leak. + // boost::python::incref(P); + // This agrees with the sample code found here: + // http://mail.python.org/pipermail/cplusplus-sig/2008-October/013825.html + return P; + } + + static bool isDimensionValid(int requestedSize, int sizeAtCompileTime, int maxSizeAtCompileTime) + { + bool valid = true; + if(sizeAtCompileTime == Eigen::Dynamic) + { + // Check for dynamic fixed size + // http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html#TutorialMatrixOptTemplParams + if(!(maxSizeAtCompileTime == Eigen::Dynamic || requestedSize <= maxSizeAtCompileTime)) + { + valid = false; + } + } + else if(sizeAtCompileTime != requestedSize) + { + valid = false; + } + return valid; + } + + static void checkMatrixSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int rows = PyArray_DIM(obj_ptr, 0); + int cols = PyArray_DIM(obj_ptr, 1); + + bool rowsValid = isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime); + bool colsValid = isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime); + if(!rowsValid || !colsValid) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkRowVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int cols) + { + if(!isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkColumnVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int rows) + { + // Check if the type can accomidate one column. + if(ColsAtCompileTime == Eigen::Dynamic || ColsAtCompileTime == 1) + { + if(!isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + else + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + + } + + static void checkVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int size = PyArray_DIM(obj_ptr, 0); + + // If the number of rows is fixed at 1, assume that is the sense of the vector. + // Otherwise, assume it is a column. + if(RowsAtCompileTime == 1) + { + checkRowVectorSizes(obj_ptr, size); + } + else + { + checkColumnVectorSizes(obj_ptr, size); + } + } + + + static void* convertible(PyObject *obj_ptr) + { + // Check for a null pointer. + if(!obj_ptr) + { + //THROW_TYPE_ERROR("PyObject pointer was null"); + return 0; + } + + // Make sure this is a numpy array. + if (!PyArray_Check(obj_ptr)) + { + //THROW_TYPE_ERROR("Conversion is only defined for numpy array and matrix types"); + return 0; + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + // Check the type of the array. + int npyType = getNpyType(array_ptr); + + if(!TypeToNumPy::canConvert(npyType)) + { + //THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + // << ". Mismatched types."); + return 0; + } + + + + // Check the array dimensions. + int nd = PyArray_NDIM(array_ptr); + + if(nd != 1 && nd != 2) + { + THROW_TYPE_ERROR("Conversion is only valid for arrays with 1 or 2 dimensions. Argument has " << nd << " dimensions"); + } + + if(nd == 1) + { + checkVectorSizes(array_ptr); + } + else + { + // Two-dimensional matrix type. + checkMatrixSizes(array_ptr); + } + + + return obj_ptr; + } + + + static void construct(PyObject *obj_ptr, boost::python::converter::rvalue_from_python_stage1_data *data) + { + boost::python::converter::rvalue_from_python_storage * matData = reinterpret_cast * >(data); + void* storage = matData->storage.bytes; + + // Make sure storage is 16byte aligned. With help from code from Memory.h + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + matrix_t * Mp = new (aligned) matrix_t(); + // Stash the memory chunk pointer for later use by boost.python + // This signals boost::python that the new value must be deleted eventually + data->convertible = storage; + + + // std::cout << "Creating aligned pointer " << aligned << " from storage " << storage << std::endl; + // std::cout << "matrix size: " << sizeof(matrix_t) << std::endl; + // std::cout << "referent size: " << boost::python::detail::referent_size< matrix_t & >::value << std::endl; + // std::cout << "sizeof(storage): " << sizeof(matData->storage) << std::endl; + // std::cout << "sizeof(bytes): " << sizeof(matData->storage.bytes) << std::endl; + + + + matrix_t & M = *Mp; + + if (!PyArray_Check(obj_ptr)) + { + THROW_TYPE_ERROR("construct is only defined for numpy array and matrix types"); + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + int nd = PyArray_NDIM(array_ptr); + if(nd == 1) + { + int size = PyArray_DIM(array_ptr, 0); + // This is a vector type + if(RowsAtCompileTime == 1) + { + // Row Vector + M.resize(1,size); + } + else + { + // Column Vector + M.resize(size,1); + } + numpyTypeDemuxer< CopyNumpyToEigenVector >(&M, array_ptr); + } + else + { + int rows = PyArray_DIM(array_ptr, 0); + int cols = PyArray_DIM(array_ptr, 1); + + M.resize(rows,cols); + numpyTypeDemuxer< CopyNumpyToEigenMatrix >(&M, array_ptr); + } + + + + + } + + + // The registration function. + static void register_converter() + { + boost::python::to_python_converter(); + boost::python::converter::registry::push_back( + &NumpyEigenConverter::convertible, + &NumpyEigenConverter::construct, + boost::python::type_id()); + + } + +}; + + + + +#endif /* NUMPY_EIGEN_CONVERTER_HPP */ diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/boost_python_headers.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/boost_python_headers.hpp new file mode 100755 index 000000000..5499d2917 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/boost_python_headers.hpp @@ -0,0 +1,250 @@ +/** + * @file boost_python_headers.hpp + * @author Paul Furgale + * @date Mon Dec 12 10:36:03 2011 + * + * @brief A header that specializes boost-python to work with fixed-sized Eigen types. + * + * The original version of this library did not include these specializations and this caused + * assert failures when running on Ubuntu 10.04 32-bit. More information about fixed-size + * vectorizable types in Eigen is available here: + * http://eigen.tuxfamily.org/dox-devel/TopicFixedSizeVectorizable.html + * + * This code has been tested on Ubunutu 10.04 64 and 32 bit, OSX Snow Leopard and OSX Lion. + * + * This code is derived from boost/python/converter/arg_from_python.hpp + * Copyright David Abrahams 2002. + * Distributed under the Boost Software License, Version 1.0. (See http://www.boost.org/LICENSE_1_0.txt) + * + */ +#ifndef NUMPY_EIGEN_CONVERTERS_HPP +#define NUMPY_EIGEN_CONVERTERS_HPP + +#include +#include +#include +#include +#include +#include + + +namespace boost { namespace python { namespace detail { + template + struct referent_size; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix& > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix const & > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + + }}} + +namespace boost { namespace python { namespace converter { + + + template + struct rvalue_from_python_data< Eigen::Matrix const &> : rvalue_from_python_storage< Eigen::Matrix const & > + { + typedef typename Eigen::Matrix T; +# if (!defined(__MWERKS__) || __MWERKS__ >= 0x3000) \ + && (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= 245) \ + && (!defined(__DECCXX_VER) || __DECCXX_VER > 60590014) \ + && !defined(BOOST_PYTHON_SYNOPSIS) /* Synopsis' OpenCXX has trouble parsing this */ + // This must always be a POD struct with m_data its first member. + BOOST_STATIC_ASSERT(BOOST_PYTHON_OFFSETOF(rvalue_from_python_storage,stage1) == 0); +# endif + + // The usual constructor + rvalue_from_python_data(rvalue_from_python_stage1_data const & _stage1) + { + this->stage1 = _stage1; + } + + + // This constructor just sets m_convertible -- used by + // implicitly_convertible<> to perform the final step of the + // conversion, where the construct() function is already known. + rvalue_from_python_data(void* convertible) + { + this->stage1.convertible = convertible; + } + + // Destroys any object constructed in the storage. + ~rvalue_from_python_data() + { + // Realign the pointer and destroy + if (this->stage1.convertible == this->storage.bytes) + { + void * storage = reinterpret_cast(this->storage.bytes); + T * aligned = reinterpret_cast(reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16)); + + //std::cout << "Destroying " << (void*)aligned << std::endl; + aligned->T::~T(); + } + } + private: + typedef typename add_reference::type>::type ref_type; + }; + + + + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const & > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + }}} + + +#endif /* NUMPY_EIGEN_CONVERTERS_HPP */ diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/copy_routines.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/copy_routines.hpp new file mode 100755 index 000000000..b112a7426 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/copy_routines.hpp @@ -0,0 +1,149 @@ +#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP +#define NUMPY_EIGEN_COPY_ROUTINES_HPP + + +template +struct CopyNumpyToEigenMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + (*M_)(r,c) = static_cast(*p); + } + } + } + +}; + +template +struct CopyEigenToNumpyMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + *p = static_cast((*M_)(r,c)); + } + } + } + +}; + +template +struct CopyEigenToNumpyVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + *p = static_cast((*M_)(i)); + } + } + +}; + + +template +struct CopyNumpyToEigenVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + (*M_)(i) = static_cast(*p); + } + } + +}; + + + + +// Crazy syntax in this function was found here: +// http://stackoverflow.com/questions/1840253/c-template-member-function-of-template-class-called-from-template-function/1840318#1840318 +template< typename FUNCTOR_T> +inline void numpyTypeDemuxer(typename FUNCTOR_T::matrix_t * M, NPE_PY_ARRAY_OBJECT * P) +{ + FUNCTOR_T f; + + int npyType = getNpyType(P); + switch(npyType) + { + case NPY_BOOL: + f.template exec(M,P); + break; + case NPY_BYTE: + f.template exec(M,P); + break; + case NPY_UBYTE: + f.template exec(M,P); + break; + case NPY_SHORT: + f.template exec(M,P); + break; + case NPY_USHORT: + f.template exec(M,P); + break; + case NPY_INT: + f.template exec(M,P); + break; + case NPY_UINT: + f.template exec(M,P); + break; + case NPY_LONG: + f.template exec(M,P); + break; + case NPY_ULONG: + f.template exec(M,P); + break; + case NPY_LONGLONG: + f.template exec(M,P); + break; + case NPY_ULONGLONG: + f.template exec(M,P); + break; + case NPY_FLOAT: + f.template exec(M,P); + break; + case NPY_DOUBLE: + f.template exec(M,P); + break; + case NPY_LONGDOUBLE: + f.template exec(M,P); + break; + default: + THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType)); + } +} + + +#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */ diff --git a/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/type_traits.hpp b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/type_traits.hpp new file mode 100755 index 000000000..97e4bb5a0 --- /dev/null +++ b/gtsam/3rdparty/numpy_eigen/include/numpy_eigen/type_traits.hpp @@ -0,0 +1,191 @@ +#ifndef NUMPY_EIGEN_TYPE_TRAITS_HPP +#define NUMPY_EIGEN_TYPE_TRAITS_HPP + +#define THROW_TYPE_ERROR(msg) \ + { \ + std::stringstream type_error_ss; \ + type_error_ss << msg; \ + PyErr_SetString(PyExc_TypeError, type_error_ss.str().c_str()); \ + throw boost::python::error_already_set(); \ + } + + +//////////////////////////////////////////////// +// TypeToNumPy +// Defines helper functions based on the Eigen3 matrix type that +// decide what conversions can happen Eigen3 --> NumPy +// Also, converts a type to a NumPy enum. +template struct TypeToNumPy; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_INT }; + static const char * npyString() { return "NPY_INT"; } + static const char * typeString() { return "int"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_LONG }; + static const char * npyString() { return "NPY_LONG"; } + static const char * typeString() { return "long"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UINT64 }; + static const char * npyString() { return "NPY_UINT64"; } + static const char * typeString() { return "uint64"; } + static bool canConvert(int type) + { + return type == NPY_UINT8 || type == NPY_USHORT || + type == NPY_UINT16 || type == NPY_UINT32 || + type == NPY_ULONG || type == NPY_ULONGLONG || + type == NPY_UINT64; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UBYTE }; + static const char * npyString() { return "NPY_UBYTE"; } + static const char * typeString() { return "unsigned char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_BYTE }; + static const char * npyString() { return "NPY_BYTE"; } + static const char * typeString() { return "char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_FLOAT }; + static const char * npyString() { return "NPY_FLOAT"; } + static const char * typeString() { return "float"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_DOUBLE }; + static const char * npyString() { return "NPY_DOUBLE"; } + static const char * typeString() { return "double"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_DOUBLE || type == NPY_LONG; + } +}; + + +inline int getNpyType(PyObject * obj_ptr){ + return PyArray_ObjectType(obj_ptr, 0); +} + +#ifdef NPY_1_7_API_VERSION +inline int getNpyType(PyArrayObject * obj_ptr){ + PyArray_Descr * descr = PyArray_MinScalarType(obj_ptr); + if (descr == NULL){ + THROW_TYPE_ERROR("Unsupported type: PyArray_MinScalarType returned null!"); + } + return descr->type_num; +} +#endif + +inline const char * npyTypeToString(int npyType) +{ + switch(npyType) + { + case NPY_BOOL: + return "NPY_BOOL"; + case NPY_BYTE: + return "NPY_BYTE"; + case NPY_UBYTE: + return "NPY_UBYTE"; + case NPY_SHORT: + return "NPY_SHORT"; + case NPY_USHORT: + return "NPY_USHORT"; + case NPY_INT: + return "NPY_INT"; + case NPY_UINT: + return "NPY_UINT"; + case NPY_LONG: + return "NPY_LONG"; + case NPY_ULONG: + return "NPY_ULONG"; + case NPY_LONGLONG: + return "NPY_LONGLONG"; + case NPY_ULONGLONG: + return "NPY_ULONGLONG"; + case NPY_FLOAT: + return "NPY_FLOAT"; + case NPY_DOUBLE: + return "NPY_DOUBLE"; + case NPY_LONGDOUBLE: + return "NPY_LONGDOUBLE"; + case NPY_CFLOAT: + return "NPY_CFLOAT"; + case NPY_CDOUBLE: + return "NPY_CDOUBLE"; + case NPY_CLONGDOUBLE: + return "NPY_CLONGDOUBLE"; + case NPY_OBJECT: + return "NPY_OBJECT"; + case NPY_STRING: + return "NPY_STRING"; + case NPY_UNICODE: + return "NPY_UNICODE"; + case NPY_VOID: + return "NPY_VOID"; + case NPY_NTYPES: + return "NPY_NTYPES"; + case NPY_NOTYPE: + return "NPY_NOTYPE"; + case NPY_CHAR: + return "NPY_CHAR"; + default: + return "Unknown type"; + } +} + +inline std::string npyArrayTypeString(NPE_PY_ARRAY_OBJECT * obj_ptr) +{ + std::stringstream ss; + int nd = PyArray_NDIM(obj_ptr); + ss << "numpy.array<" << npyTypeToString(getNpyType(obj_ptr)) << ">["; + if(nd > 0) + { + ss << PyArray_DIM(obj_ptr, 0); + for(int i = 1; i < nd; i++) + { + ss << ", " << PyArray_DIM(obj_ptr, i); + } + } + ss << "]"; + return ss.str(); +} + + +#endif /* NUMPY_EIGEN_TYPE_TRAITS_HPP */ diff --git a/python/.gitignore b/python/.gitignore new file mode 100644 index 000000000..d16386367 --- /dev/null +++ b/python/.gitignore @@ -0,0 +1 @@ +build/ \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 000000000..01141973a --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,96 @@ +# Guard to avoid breaking this code in ccmake if by accident GTSAM_PYTHON_VERSION is set to an empty string +if(GTSAM_PYTHON_VERSION STREQUAL "") + set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version" FORCE) +endif() + +# The code below allows to clear the PythonLibs cache if we change GTSAM_PYTHON_VERSION +# Inspired from the solution found here: http://blog.bethcodes.com/cmake-tips-tricks-drop-down-list +if(NOT DEFINED GTSAM_LAST_PYTHON_VERSION) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Python version used in the last build") + mark_as_advanced(FORCE GTSAM_LAST_PYTHON_VERSION) +endif() +if(NOT (${GTSAM_PYTHON_VERSION} MATCHES ${GTSAM_LAST_PYTHON_VERSION})) + unset(PYTHON_INCLUDE_DIR CACHE) + unset(PYTHON_INCLUDE_DIR2 CACHE) + unset(PYTHON_LIBRARY CACHE) + unset(PYTHON_LIBRARY_DEBUG CACHE) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Updating python version used in the last build" FORCE) +endif() + +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + # Search the default version. + find_package(PythonInterp) + find_package(PythonLibs) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION}) + find_package(PythonLibs ${GTSAM_PYTHON_VERSION}) +endif() + +# Find NumPy C-API -- this is part of the numpy package +find_package(NumPy) + +# Compose strings used to specify the boost python version. They will be empty if we want to use the defaut +if(NOT GTSAM_PYTHON_VERSION STREQUAL "Default") + string(REPLACE "." "" BOOST_PYTHON_VERSION_SUFFIX ${GTSAM_PYTHON_VERSION}) # Remove '.' from version + string(SUBSTRING ${BOOST_PYTHON_VERSION_SUFFIX} 0 2 BOOST_PYTHON_VERSION_SUFFIX) # Trim version number to 2 digits + set(BOOST_PYTHON_VERSION_SUFFIX "-py${BOOST_PYTHON_VERSION_SUFFIX}") # Append '-py' prefix + string(TOUPPER ${BOOST_PYTHON_VERSION_SUFFIX} BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE) # Get uppercase string +else() + set(BOOST_PYTHON_VERSION_SUFFIX "") + set(BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE "") +endif() + +# Find Boost Python +find_package(Boost COMPONENTS python${BOOST_PYTHON_VERSION_SUFFIX}) + +if(Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND AND PYTHONLIBS_FOUND AND NUMPY_FOUND) + # Build library + include_directories(${NUMPY_INCLUDE_DIRS}) + include_directories(${PYTHON_INCLUDE_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + include_directories(${CMAKE_SOURCE_DIR}/gtsam/3rdparty/numpy_eigen/include/) + + # Build the python module library + add_subdirectory(handwritten) + + # Create and invoke setup.py, see https://bloerg.net/2012/11/10/cmake-and-distutils.html + set(SETUP_PY_IN "${CMAKE_CURRENT_SOURCE_DIR}/setup.py.in") + set(SETUP_PY "${CMAKE_CURRENT_BINARY_DIR}/setup.py") + + # Hacky way to figure out install folder - valid for Linux & Mac + # default pattern: prefix/lib/pythonX.Y/site-packages from https://docs.python.org/2/install/ + SET(PY_INSTALL_FOLDER "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages") + + configure_file(${SETUP_PY_IN} ${SETUP_PY}) + + # TODO(frank): possibly support a different prefix a la matlab wrapper + install(CODE "execute_process(WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${PYTHON_EXECUTABLE} setup.py -v install --prefix ${CMAKE_INSTALL_PREFIX})") +else() + # Disable python module if we didn't find required libraries + # message will print at end of main CMakeLists.txt + SET(GTSAM_PYTHON_WARNINGS "Python dependencies not found - Python module will not be built. Set GTSAM_BUILD_PYTHON to 'Off' to disable this warning. Details:") + + if(NOT PYTHONLIBS_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default PythonLibs not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- PythonLibs version ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + if(NOT NUMPY_FOUND) + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Numpy not found") + endif() + + if(NOT Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default Boost python not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Boost Python for python ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + # make available at top-level + SET(GTSAM_PYTHON_WARNINGS ${GTSAM_PYTHON_WARNINGS} PARENT_SCOPE) + +endif() diff --git a/python/README.md b/python/README.md new file mode 100644 index 000000000..f1c218cfb --- /dev/null +++ b/python/README.md @@ -0,0 +1,18 @@ +Python Wrapper and Packaging +============================ + +This directory contains the basic setup script and directory structure for the gtsam python module. +During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run. + +* The handwritten module source files are compiled and linked with Boost Python, generating a shared + library which can then be imported by python +* A setup.py script is configured from setup.py.in +* The gtsam packages 'gtsam', 'gtsam_utils', 'gtsam_examples', and 'gtsam_tests' are installed into + the site-packages folder within the (possibly non-default) installation prefix folder. If + installing to a non-standard prefix, make sure that _prefix_/lib/pythonX.Y/site-packages is + present in your $PYTHONPATH + +The target version of Python to create the module can be set by defining GTSAM_PYTHON_VERSION to 'X.Y' (Example: 2.7 or 3.4), or 'Default' if you want to use the default python installed in your system. Note that if you specify a target version of python, you should also have the correspondening Boost +Python version installed (Example: libboost_python-py27.so or libboost_python-py34.so on Linux). +If you're using the default version, your default Boost Python library (Example: libboost_python.so on Linux) should correspond to the default python version in your system. + diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py new file mode 100644 index 000000000..f9e40517d --- /dev/null +++ b/python/gtsam/__init__.py @@ -0,0 +1 @@ +from ._libgtsam_python import * diff --git a/python/gtsam_examples/OdometryExample.py b/python/gtsam_examples/OdometryExample.py new file mode 100644 index 000000000..0800bab4e --- /dev/null +++ b/python/gtsam_examples/OdometryExample.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +from __future__ import print_function +import gtsam +import numpy as np + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + +# Add odometry factors +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# For simplicity, we will use the same noise model for each odometry factor +odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) +graph.print("\nFactor Graph:\n") + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) +initial.print("\nInitial Estimate:\n") + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +result.print("\nFinal Result:\n") diff --git a/python/gtsam_examples/SFMdata.py b/python/gtsam_examples/SFMdata.py new file mode 100644 index 000000000..21a438226 --- /dev/null +++ b/python/gtsam_examples/SFMdata.py @@ -0,0 +1,32 @@ + + # A structure-from-motion example with landmarks + # - The landmarks form a 10 meter cube + # - The robot rotates around the landmarks, always facing towards the cube + +import gtsam +import numpy as np + +def createPoints(): + # Create the set of ground-truth landmarks + points = [gtsam.Point3(10.0,10.0,10.0), + gtsam.Point3(-10.0,10.0,10.0), + gtsam.Point3(-10.0,-10.0,10.0), + gtsam.Point3(10.0,-10.0,10.0), + gtsam.Point3(10.0,10.0,-10.0), + gtsam.Point3(-10.0,10.0,-10.0), + gtsam.Point3(-10.0,-10.0,-10.0), + gtsam.Point3(10.0,-10.0,-10.0)] + return points + +def createPoses(): + # Create the set of ground-truth poses + radius = 30.0 + angles = np.linspace(0,2*np.pi,8,endpoint=False) + up = gtsam.Point3(0,0,1) + target = gtsam.Point3(0,0,0) + poses = [] + for theta in angles: + position = gtsam.Point3(radius*np.cos(theta), radius*np.sin(theta), 0.0) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up) + poses.append(camera.pose()) + return poses diff --git a/python/gtsam_examples/VisualISAM2Example.py b/python/gtsam_examples/VisualISAM2Example.py new file mode 100644 index 000000000..4d7889e9b --- /dev/null +++ b/python/gtsam_examples/VisualISAM2Example.py @@ -0,0 +1,130 @@ +from __future__ import print_function + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import time # for sleep() + +import gtsam +from gtsam_examples import SFMdata +import gtsam_utils + +def visual_ISAM2_plot(poses, points, result): + # VisualISAMPlot plots current state of ISAM2 object + # Author: Ellon Paiva + # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert + + # Declare an id for the figure + fignum = 0; + + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + plt.cla() + + # Plot points + # Can't use data because current frame might not see all points + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow + # gtsam.plot3DPoints(result, [], marginals); + gtsam_utils.plot3DPoints(fignum, result, 'rx'); + + # Plot cameras + M = 0; + while result.exists(int(gtsam.Symbol('x',M))): + ii = int(gtsam.Symbol('x',M)); + pose_i = result.pose3_at(ii); + gtsam_utils.plotPose3(fignum, pose_i, 10); + + M = M + 1; + + # draw + ax.set_xlim3d(-40, 40) + ax.set_ylim3d(-40, 40) + ax.set_zlim3d(-40, 40) + plt.ion() + plt.show() + plt.draw() + +def visual_ISAM2_example(): + # Define the camera calibration parameters + K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses() + + # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization + # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter + # structure is available that allows the user to set various properties, such as the relinearization threshold + # and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + # will approach the batch result. + parameters = gtsam.ISAM2Params() + parameters.relinearize_threshold = 0.01 + parameters.relinearize_skip = 1 + isam = gtsam.ISAM2(parameters) + + # Create a Factor Graph and Values to hold the new data + graph = gtsam.NonlinearFactorGraph() + initialEstimate = gtsam.Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + + # Add factors for each landmark observation + for j, point in enumerate(points): + camera = gtsam.PinholeCameraCal3_S2(pose, K) + measurement = camera.project(point) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(gtsam.Symbol('x', i)), int(gtsam.Symbol('l', j)), K)) + + # Add an initial guess for the current pose + # Intentionally initialize the variables off from the ground truth + initialEstimate.insert(int(gtsam.Symbol('x', i)), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + # If this is the first iteration, add a prior on the first pose to set the coordinate frame + # and a prior on the first landmark to set the scale + # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + # adding it to iSAM. + if( i == 0): + # Add a prior on pose x0 + poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(gtsam.PriorFactorPose3(int(gtsam.Symbol('x', 0)), poses[0], poseNoise)) + + # Add a prior on landmark l0 + pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + graph.push_back(gtsam.PriorFactorPoint3(int(gtsam.Symbol('l', 0)), points[0], pointNoise)) # add directly to graph + + # Add initial guesses to all observed landmarks + # Intentionally initialize the variables off from the ground truth + for j, point in enumerate(points): + initialEstimate.insert(int(gtsam.Symbol('l', j)), point + gtsam.Point3(-0.25, 0.20, 0.15)); + else: + # Update iSAM with the new factors + isam.update(graph, initialEstimate) + # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + # If accuracy is desired at the expense of time, update(*) can be called additional times + # to perform multiple optimizer iterations every step. + isam.update() + currentEstimate = isam.calculate_estimate(); + print( "****************************************************" ) + print( "Frame", i, ":" ) + for j in range(i+1): + print( gtsam.Symbol('x',j) ) + print( currentEstimate.pose3_at(int(gtsam.Symbol('x',j))) ) + + for j in range(len(points)): + print( gtsam.Symbol('l',j) ) + print( currentEstimate.point3_at(int(gtsam.Symbol('l',j))) ) + + visual_ISAM2_plot(poses, points, currentEstimate); + time.sleep(1) + + # Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + +if __name__ == '__main__': + visual_ISAM2_example() diff --git a/python/gtsam_examples/__init__.py b/python/gtsam_examples/__init__.py new file mode 100644 index 000000000..f9d032d54 --- /dev/null +++ b/python/gtsam_examples/__init__.py @@ -0,0 +1,2 @@ +from . import SFMdata +from . import VisualISAM2Example diff --git a/python/gtsam_tests/__init__.py b/python/gtsam_tests/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/python/gtsam_tests/__init__.py @@ -0,0 +1 @@ + diff --git a/python/gtsam_tests/testPoint2.py b/python/gtsam_tests/testPoint2.py new file mode 100644 index 000000000..80a6f6cbf --- /dev/null +++ b/python/gtsam_tests/testPoint2.py @@ -0,0 +1,13 @@ +import unittest +from gtsam import * + +#https://docs.python.org/2/library/unittest.html +class TestPoint2(unittest.TestCase): + def setUp(self): + self.point = Point2() + + def test_constructor(self): + pass + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_utils/__init__.py b/python/gtsam_utils/__init__.py new file mode 100644 index 000000000..0ef2dfcd3 --- /dev/null +++ b/python/gtsam_utils/__init__.py @@ -0,0 +1 @@ +from ._plot import * diff --git a/python/gtsam_utils/_plot.py b/python/gtsam_utils/_plot.py new file mode 100644 index 000000000..f1603bbf5 --- /dev/null +++ b/python/gtsam_utils/_plot.py @@ -0,0 +1,59 @@ +import numpy as _np +import matplotlib.pyplot as _plt +from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3(fignum, point, linespec): + fig = _plt.figure(fignum) + ax = fig.gca(projection='3d') + ax.plot([point.x()],[point.y()],[point.z()], linespec) + + +def plot3DPoints(fignum, values, linespec, marginals=None): + # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances + # Finds all the Point3 objects in the given Values object and plots them. + # If a Marginals object is given, this function will also plot marginal + # covariance ellipses for each point. + + keys = values.keys() + + # Plot points and covariance matrices + for key in keys: + try: + p = values.point3_at(key); + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plotPoint3(p, linespec, P); + # else + plotPoint3(fignum, p, linespec); + except RuntimeError: + continue + #I guess it's not a Point3 + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = _plt.figure(fignum) + ax = fig.gca(projection='3d') + + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + C = pose.translation().vector() + + # draw the camera axes + xAxis = C+gRp[:,0]*axisLength + L = _np.append(C[_np.newaxis], xAxis[_np.newaxis], axis=0) + ax.plot(L[:,0],L[:,1],L[:,2],'r-') + + yAxis = C+gRp[:,1]*axisLength + L = _np.append(C[_np.newaxis], yAxis[_np.newaxis], axis=0) + ax.plot(L[:,0],L[:,1],L[:,2],'g-') + + zAxis = C+gRp[:,2]*axisLength + L = _np.append(C[_np.newaxis], zAxis[_np.newaxis], axis=0) + ax.plot(L[:,0],L[:,1],L[:,2],'b-') + + # # plot the covariance + # if (nargin>2) && (~isempty(P)) + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + # gtsam.covarianceEllipse3D(C,gPp); + # end \ No newline at end of file diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt new file mode 100644 index 000000000..9d4f9151a --- /dev/null +++ b/python/handwritten/CMakeLists.txt @@ -0,0 +1,30 @@ +# get subdirectories list +subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR}) + +# get the sources needed to compile gtsam python module +set(gtsam_python_srcs "") +foreach(subdir ${SUBDIRS}) + file(GLOB ${subdir}_src "${subdir}/*.cpp") + list(APPEND gtsam_python_srcs ${${subdir}_src}) +endforeach() + +# Create the library +add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) +set_target_properties(gtsam_python PROPERTIES + OUTPUT_NAME gtsam_python + SKIP_BUILD_RPATH TRUE + CLEAN_DIRECT_OUTPUT 1 +) +target_link_libraries(gtsam_python + ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} + ${PYTHON_LIBRARY} gtsam) + +# Cause the library to be output in the correct directory. +# TODO: Change below to work on different systems (currently works only with Linux) +add_custom_command( + OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + DEPENDS gtsam_python + COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" +) +add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so) \ No newline at end of file diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp new file mode 100644 index 000000000..1c3582813 --- /dev/null +++ b/python/handwritten/base/FastVector.cpp @@ -0,0 +1,37 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps FastVector instances to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/FastVector.h" +#include "gtsam/base/types.h" // for Key definition + +using namespace boost::python; +using namespace gtsam; + +void exportFastVectors(){ + + typedef FastVector KeyVector; + + class_("KeyVector") + .def(vector_indexing_suite()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp new file mode 100644 index 000000000..da8382d71 --- /dev/null +++ b/python/handwritten/exportgtsam.cpp @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports the python module + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#include + +// Base +void exportFastVectors(); + +// Geometry +void exportPoint2(); +void exportPoint3(); +void exportRot2(); +void exportRot3(); +void exportPose2(); +void exportPose3(); +void exportPinholeBaseK(); +void exportPinholeCamera(); +void exportCal3_S2(); + +// Inference +void exportSymbol(); + +// Linear +void exportNoiseModels(); + +// Nonlinear +void exportValues(); +void exportNonlinearFactor(); +void exportNonlinearFactorGraph(); +void exportLevenbergMarquardtOptimizer(); +void exportISAM2(); + +// Slam +void exportPriorFactors(); +void exportBetweenFactors(); +void exportGenericProjectionFactor(); + +// Utils (or Python wrapper specific functions) +void registerNumpyEigenConversions(); + +//-----------------------------------// + +BOOST_PYTHON_MODULE(_libgtsam_python){ + + // NOTE: We need to call import_array1() instead of import_array() to support both python 2 + // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function + // returning void, and import_array() is a macro that when expanded for python 3, adds + // a 'return __null' statement to that function. For more info check files: + // boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file). + // Should be the first thing to be done + import_array1(); + + registerNumpyEigenConversions(); + + exportFastVectors(); + + exportPoint2(); + exportPoint3(); + exportRot2(); + exportRot3(); + exportPose2(); + exportPose3(); + exportPinholeBaseK(); + exportPinholeCamera(); + exportCal3_S2(); + + exportSymbol(); + + exportNoiseModels(); + + exportValues(); + exportNonlinearFactor(); + exportNonlinearFactorGraph(); + exportLevenbergMarquardtOptimizer(); + exportISAM2(); + + exportPriorFactors(); + exportBetweenFactors(); + exportGenericProjectionFactor(); +} \ No newline at end of file diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp new file mode 100644 index 000000000..339cd6a3c --- /dev/null +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Cal3_S2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3) + +// Function pointers to desambiguate Cal3_S2::calibrate calls +Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate; +Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate; + +void exportCal3_S2(){ + +class_("Cal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Cal3_S2::print, print_overloads(args("s"))) + .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol"))) + .def("fx",&Cal3_S2::fx) + .def("fy",&Cal3_S2::fy) + .def("skew",&Cal3_S2::skew) + .def("px",&Cal3_S2::px) + .def("py",&Cal3_S2::py) + .def("principal_point",&Cal3_S2::principalPoint) + .def("vector",&Cal3_S2::vector) + .def("k",&Cal3_S2::K) + .def("matrix",&Cal3_S2::matrix) + .def("matrix_inverse",&Cal3_S2::matrix_inverse) + .def("uncalibrate",&Cal3_S2::uncalibrate, uncalibrate_overloads()) + .def("calibrate",calibrate1, calibrate_overloads()) + .def("calibrate",calibrate2) + .def("between",&Cal3_S2::between, between_overloads()) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp new file mode 100644 index 000000000..da98783e2 --- /dev/null +++ b/python/handwritten/geometry/PinholeBaseK.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; + +// Wrapper on PinholeBaseK because it has pure virtual method calibration() +struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper +{ + const Cal3_S2 & calibration () const { + return this->get_override("calibration")(); + } +}; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3) + +// Function pointers to desambiguate project() calls +Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; + +// function pointers to desambiguate range() calls +double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; + +void exportPinholeBaseK(){ + + class_("PinholeBaseKCal3_S2", no_init) + .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy()) + .def("project", project1) + .def("project", project2, project_overloads()) + .def("project", project3, project_overloads()) + .def("backproject", &PinholeBaseKCal3_S2::backproject) + .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("range", range3, range_overloads()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp new file mode 100644 index 000000000..21ffcdeb7 --- /dev/null +++ b/python/handwritten/geometry/PinholeCamera.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; +typedef PinholeCamera PinholeCameraCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, PinholeCameraCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PinholeCameraCal3_S2::equals, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Lookat_overloads, PinholeCameraCal3_S2::Lookat, 3, 4) + +void exportPinholeCamera(){ + +class_ >("PinholeCameraCal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &PinholeCameraCal3_S2::print, print_overloads(args("s"))) + .def("equals", &PinholeCameraCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("pose", &PinholeCameraCal3_S2::pose, return_value_policy()) + // We don't need to define calibration() here because it's already defined as virtual in the base class PinholeBaseKCal3_S2 + // .def("calibration", &PinholeCameraCal3_S2::calibration, return_value_policy()) + .def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads()) + .staticmethod("Lookat") +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp new file mode 100644 index 000000000..7af3f8cf6 --- /dev/null +++ b/python/handwritten/geometry/Point2.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) + +void exportPoint2(){ + + class_("Point2", init<>()) + .def(init()) + .def(init()) + .def("identity", &Point2::identity) + .def("dist", &Point2::dist) + .def("distance", &Point2::distance) + .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point2::norm) + .def("print", &Point2::print, print_overloads(args("s"))) + .def("unit", &Point2::unit) + .def("vector", &Point2::vector) + .def("x", &Point2::x) + .def("y", &Point2::y) + .def(self * other()) // __mult__ + .def(other() * self) // __mult__ + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp new file mode 100644 index 000000000..664b4ffda --- /dev/null +++ b/python/handwritten/geometry/Point3.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) + +void exportPoint3(){ + +class_("Point3") + .def(init<>()) + .def(init()) + .def(init()) + .def("identity", &Point3::identity) + .staticmethod("identity") + .def("add", &Point3::add) + .def("cross", &Point3::cross) + .def("dist", &Point3::dist) + .def("distance", &Point3::distance) + .def("dot", &Point3::dot) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point3::norm) + .def("normalize", &Point3::normalize) + .def("print", &Point3::print, print_overloads(args("s"))) + .def("sub", &Point3::sub) + .def("vector", &Point3::vector) + .def("x", &Point3::x) + .def("y", &Point3::y) + .def("z", &Point3::z) + .def(self * other()) + .def(other() * self) + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp new file mode 100644 index 000000000..4f402df7e --- /dev/null +++ b/python/handwritten/geometry/Pose2.cpp @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) + +// Manually wrap + +void exportPose2(){ + + // double (Pose2::*range1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::range; + // double (Pose2::*range2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::range; + + // Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + // Rot2 (Pose2::*bearing2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + + class_("Pose2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose2::print, print_overloads(args("s"))) + + .def("equals", &Pose2::equals, equals_overloads(args("pose","tol"))) + // .def("inverse", &Pose2::inverse) + // .def("compose", &Pose2::compose, compose_overloads(args("p2", "H1", "H2"))) + // .def("between", &Pose2::between, between_overloads(args("p2", "H1", "H2"))) + // .def("dim", &Pose2::dim) + // .def("retract", &Pose2::retract) + + .def("transform_to", &Pose2::transform_to, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_from", &Pose2::transform_from, + transform_to_overloads(args("point", "H1", "H2"))) + + .def("x", &Pose2::x) + .def("y", &Pose2::y) + .def("theta", &Pose2::theta) + // See documentation on call policy for more information + // https://wiki.python.org/moin/boost.python/CallPolicy + .def("t", &Pose2::t, return_value_policy()) + .def("r", &Pose2::r, return_value_policy()) + .def("translation", &Pose2::translation, return_value_policy()) + .def("rotation", &Pose2::rotation, return_value_policy()) + + // .def("bearing", bearing1, bearing_overloads()) + // .def("bearing", bearing2, bearing_overloads()) + + // Function overload example + // .def("range", range1, range_overloads()) + // .def("range", range2, range_overloads()) + + + .def("Expmap", &Pose2::Expmap) + .staticmethod("Expmap") + + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp new file mode 100644 index 000000000..dfdfe8ad1 --- /dev/null +++ b/python/handwritten/geometry/Pose3.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3) + +void exportPose3(){ + + // function pointers to desambiguate transform_to() calls + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; + Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; + // function pointers to desambiguate compose() calls + Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose; + Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose; + // function pointers to desambiguate between() calls + Pose3 (Pose3::*between1)(const Pose3 &g) const = &Pose3::between; + Pose3 (Pose3::*between2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::between; + // function pointers to desambiguate range() calls + double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; + double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + + class_("Pose3") + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def("rotation", &Pose3::rotation, return_value_policy()) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + .def("compose", compose1) + .def("compose", compose2, compose_overloads()) + .def("between", between1) + .def("between", between2, between_overloads()) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("bearing", &Pose3::bearing, bearing_overloads()) + ; +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp new file mode 100644 index 000000000..59b4ce4e8 --- /dev/null +++ b/python/handwritten/geometry/Rot2.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Rot2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(relativeBearing_overloads, Rot2::relativeBearing, 1, 3) + +void exportRot2(){ + + class_("Rot2", init<>()) + .def(init()) + .def("Expmap", &Rot2::Expmap) + .staticmethod("Expmap") + .def("Logmap", &Rot2::Logmap) + .staticmethod("Logmap") + .def("atan2", &Rot2::atan2) + .staticmethod("atan2") + .def("fromAngle", &Rot2::fromAngle) + .staticmethod("fromAngle") + .def("fromCosSin", &Rot2::fromCosSin) + .staticmethod("fromCosSin") + .def("fromDegrees", &Rot2::fromDegrees) + .staticmethod("fromDegrees") + .def("identity", &Rot2::identity) + .staticmethod("identity") + .def("relativeBearing", &Rot2::relativeBearing) + .staticmethod("relativeBearing") + .def("c", &Rot2::c) + .def("degrees", &Rot2::degrees) + .def("equals", &Rot2::equals, equals_overloads(args("q","tol"))) + .def("matrix", &Rot2::matrix) + .def("print", &Rot2::print, print_overloads(args("s"))) + .def("rotate", &Rot2::rotate) + .def("s", &Rot2::s) + .def("theta", &Rot2::theta) + .def("unrotate", &Rot2::unrotate) + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp new file mode 100644 index 000000000..b4551ca5c --- /dev/null +++ b/python/handwritten/geometry/Rot3.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +static Rot3 Quaternion_0(const Vector4& q) +{ + return Rot3::quaternion(q[0],q[1],q[2],q[3]); +} + +static Rot3 Quaternion_1(double w, double x, double y, double z) +{ + return Rot3::quaternion(w,x,y,z); +} + +// Prototypes used to perform overloading +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html +gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; +gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; +gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; +gtsam::Rot3 (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx; +Vector (Rot3::*quaternion_0)() const = &Rot3::quaternion; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2) + +void exportRot3(){ + + class_("Rot3") + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") + .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) + .staticmethod("Quaternion") + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") + .def("Expmap", &Rot3::Expmap) + .staticmethod("Expmap") + .def("ExpmapDerivative", &Rot3::ExpmapDerivative) + .staticmethod("ExpmapDerivative") + .def("Logmap", &Rot3::Logmap) + .staticmethod("Logmap") + .def("LogmapDerivative", &Rot3::LogmapDerivative) + .staticmethod("LogmapDerivative") + .def("Rodrigues", Rodrigues_0) + .def("Rodrigues", Rodrigues_1) + .staticmethod("Rodrigues") + .def("Rx", &Rot3::Rx) + .staticmethod("Rx") + .def("Ry", &Rot3::Ry) + .staticmethod("Ry") + .def("Rz", &Rot3::Rz) + .staticmethod("Rz") + .def("RzRyRx", RzRyRx_0, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .staticmethod("RzRyRx") + .def("identity", &Rot3::identity) + .staticmethod("identity") + .def("AdjointMap", &Rot3::AdjointMap) + .def("column", &Rot3::column) + .def("conjugate", &Rot3::conjugate) + .def("equals", &Rot3::equals, equals_overloads(args("q","tol"))) +#ifndef GTSAM_USE_QUATERNIONS + .def("localCayley", &Rot3::localCayley) + .def("retractCayley", &Rot3::retractCayley) +#endif + .def("matrix", &Rot3::matrix) + .def("print", &Rot3::print, print_overloads(args("s"))) + .def("r1", &Rot3::r1) + .def("r2", &Rot3::r2) + .def("r3", &Rot3::r3) + .def("rpy", &Rot3::rpy) + .def("slerp", &Rot3::slerp) + .def("transpose", &Rot3::transpose) + .def("xyz", &Rot3::xyz) + .def("quaternion", quaternion_0) + .def(self * self) + .def(self * other()) + .def(self * other()) + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp new file mode 100644 index 000000000..9fc5b74e7 --- /dev/null +++ b/python/handwritten/inference/Symbol.cpp @@ -0,0 +1,83 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Symbol class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include // for stringstream + +#include "gtsam/inference/Symbol.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Symbol::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Symbol::equals, 1, 2) + +// Helper function to allow building a symbol from a python string and a index. +static boost::shared_ptr makeSymbol(const std::string &str, size_t j) +{ + if(str.size() > 1) + throw std::runtime_error("string argument must have one character only"); + + return boost::make_shared(str.at(0),j); +} + +// Helper function to print the symbol as "char-and-index" in python +std::string selfToString(const Symbol & self) +{ + return (std::string)self; +} + +// Helper function to convert a Symbol to int using int() cast in python +size_t selfToKey(const Symbol & self) +{ + return self.key(); +} + +// Helper function to recover symbol's unsigned char as string +std::string chrFromSelf(const Symbol & self) +{ + std::stringstream ss; + ss << self.chr(); + return ss.str(); +} + +void exportSymbol(){ + +class_ >("Symbol") + .def(init<>()) + .def(init()) + .def("__init__", make_constructor(makeSymbol)) + .def(init()) + .def("print", &Symbol::print, print_overloads(args("s"))) + .def("equals", &Symbol::equals, equals_overloads(args("q","tol"))) + .def("key", &Symbol::key) + .def("index", &Symbol::index) + .def(self < self) + .def(self == self) + .def(self == other()) + .def(self != self) + .def(self != other()) + .def("__repr__", &selfToString) + .def("__int__", &selfToKey) + .def("chr", &chrFromSelf) +; + +} \ No newline at end of file diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp new file mode 100644 index 000000000..3453184bd --- /dev/null +++ b/python/handwritten/linear/NoiseModel.cpp @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps the noise model classes into the noiseModel module + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + + /** TODOs Summary: + * + * TODO(Ellon): Don't know yet it it's worth/needed to add 'Wrap structs' for each of the noise models. + * I think it's only worthy if we want to access virtual the virtual functions from python. + * TODO(Ellon): Wrap non-pure virtual methods of Base on BaseWrap + */ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/linear/NoiseModel.h" + +using namespace boost::python; +using namespace gtsam; +using namespace gtsam::noiseModel; + +// Wrap around pure virtual class Base. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct BaseCallback : Base, wrapper +{ + void print (const std::string & name="") const { + this->get_override("print")(); + } + bool equals (const Base & expected, double tol=1e-9) const { + return this->get_override("equals")(); + } + Vector whiten (const Vector & v) const { + return this->get_override("whiten")(); + } + Matrix Whiten (const Matrix & v) const { + return this->get_override("Whiten")(); + } + Vector unwhiten (const Vector & v) const { + return this->get_override("unwhiten")(); + } + double distance (const Vector & v) const { + return this->get_override("distance")(); + } + void WhitenSystem (std::vector< Matrix > &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const { + this->get_override("WhitenSystem")(); + } + + // TODO(Ellon): Wrap non-pure virtual methods should go here. + // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.virtual_functions_with_default_implementations + +}; + +// Overloads for named constructors. Named constructors are static, so we declare them +// using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Information_overloads, Gaussian::Information, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Covariance_overloads, Gaussian::Covariance, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Sigmas_overloads, Diagonal::Sigmas, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Variances_overloads, Diagonal::Variances, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Precisions_overloads, Diagonal::Precisions, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Sigma_overloads, Isotropic::Sigma, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Variance_overloads, Isotropic::Variance, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Precision_overloads, Isotropic::Precision, 2, 3) + + +void exportNoiseModels(){ + + // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html + std::string noiseModel_name = extract(scope().attr("__name__") + ".noiseModel"); + object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); + scope().attr("noiseModel") = noiseModel_module; + scope noiseModel_scope = noiseModel_module; + + // Then export our classes in the noiseModel scope + class_("Base") + .def("print", pure_virtual(&Base::print)) + ; + + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) + class_, bases >("Gaussian", no_init) + .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) + .staticmethod("SqrtInformation") + .def("Information",&Gaussian::Information, Gaussian_Information_overloads()) + .staticmethod("Information") + .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) + .staticmethod("Covariance") + ; + + class_, bases >("Diagonal", no_init) + .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) + .staticmethod("Sigmas") + .def("Variances",&Diagonal::Variances, Diagonal_Variances_overloads()) + .staticmethod("Variances") + .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) + .staticmethod("Precisions") + ; + + class_, bases >("Isotropic", no_init) + .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) + .staticmethod("Sigma") + .def("Variance",&Isotropic::Variance, Isotropic_Variance_overloads()) + .staticmethod("Variance") + .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) + .staticmethod("Precision") + ; + + class_, bases >("Unit", no_init) + .def("Create",&Unit::Create) + .staticmethod("Create") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..f80c5df99 --- /dev/null +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports ISAM2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/ISAM2.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(update_overloads, ISAM2::update, 0, 7) + +void exportISAM2(){ + +// TODO(Ellon): Export all properties of ISAM2Params +class_("ISAM2Params") + .add_property("relinearize_skip", &ISAM2Params::getRelinearizeSkip, &ISAM2Params::setRelinearizeSkip) + .add_property("enable_relinearization", &ISAM2Params::isEnableRelinearization, &ISAM2Params::setEnableRelinearization) + .add_property("evaluate_non_linear_error", &ISAM2Params::isEvaluateNonlinearError, &ISAM2Params::setEvaluateNonlinearError) + .add_property("factorization", &ISAM2Params::getFactorization, &ISAM2Params::setFactorization) + .add_property("cache_linearized_factors", &ISAM2Params::isCacheLinearizedFactors, &ISAM2Params::setCacheLinearizedFactors) + .add_property("enable_detailed_results", &ISAM2Params::isEnableDetailedResults, &ISAM2Params::setEnableDetailedResults) + .add_property("enable_partial_linearization_check", &ISAM2Params::isEnablePartialRelinearizationCheck, &ISAM2Params::setEnablePartialRelinearizationCheck) + // TODO(Ellon): Check if it works with FastMap; Implement properly if it doesn't. + .add_property("relinearization_threshold", &ISAM2Params::getRelinearizeThreshold, &ISAM2Params::setRelinearizeThreshold) + // TODO(Ellon): Wrap the following setters/getters: + // void setOptimizationParams (OptimizationParams optimizationParams) + // OptimizationParams getOptimizationParams () const + // void setKeyFormatter (KeyFormatter keyFormatter) + // KeyFormatter getKeyFormatter () const + // GaussianFactorGraph::Eliminate getEliminationFunction () const +; + +// TODO(Ellon): Export useful methods/properties of ISAM2Result +class_("ISAM2Result") +; + +// Function pointers for overloads in ISAM2 +Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate; + +class_("ISAM2") + .def(init()) + // TODO(Ellon): wrap all optional values of update + .def("update",&ISAM2::update, update_overloads()) + .def("calculate_estimate", calculateEstimate_0) + .def("calculate_pose3_estimate", &ISAM2::calculateEstimate, (arg("self"), arg("key")) ) + .def("value_exists", &ISAM2::valueExists) +; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp new file mode 100644 index 000000000..7c7cdf3cc --- /dev/null +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -0,0 +1,27 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +void exportLevenbergMarquardtOptimizer(){ + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) + ; + + class_("LevenbergMarquardtOptimizer", + init()) + .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) + ; +} diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..9a130d8e9 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports virtual class NonlinearFactor to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +// Wrap around pure virtual class NonlinearFactor. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct NonlinearFactorCallback : NonlinearFactor, wrapper +{ + double error (const Values & values) const { + return this->get_override("error")(values); + } + size_t dim () const { + return this->get_override("dim")(); + } + boost::shared_ptr linearize(const Values & values) const { + return this->get_override("linearize")(values); + } +}; + +void exportNonlinearFactor(){ + + class_("NonlinearFactor") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp new file mode 100644 index 000000000..f1e14deda --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports NonlinearFactorGraph class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactorGraph.h" +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1); + +void exportNonlinearFactorGraph(){ + + typedef NonlinearFactorGraph::sharedFactor sharedFactor; + + void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; + + class_("NonlinearFactorGraph", init<>()) + .def("size",&NonlinearFactorGraph::size) + .def("push_back", push_back1) + .def("add", add1) + .def("resize", &NonlinearFactorGraph::resize) + .def("empty", &NonlinearFactorGraph::empty) + .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) + ; + +} diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp new file mode 100644 index 000000000..6715fb071 --- /dev/null +++ b/python/handwritten/nonlinear/Values.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Values class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/Values.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +/** The function ValuesAt is a workaround to be able to call the correct templated version + * of Values::at. Without it, python would only try to match the last 'at' metho defined + * below. With this wrapper function we can call 'at' in python passing an extra type, + * which will define the type to be returned. Example: + * + * >>> import gtsam + * >>> v = gtsam.nonlinear.Values() + * >>> v.insert(1,gtsam.geometry.Point3()) + * >>> v.insert(2,gtsam.geometry.Rot3()) + * >>> v.insert(3,gtsam.geometry.Pose3()) + * >>> v.at(1,gtsam.geometry.Point3()) + * >>> v.at(2,gtsam.geometry.Rot3()) + * >>> v.at(3,gtsam.geometry.Pose3()) + * + * A more 'pythonic' way I think would be to not use this function and define different + * 'at' methods below using the name of the type in the function name, like: + * + * .def("point3_at", &Values::at, return_internal_reference<>()) + * .def("rot3_at", &Values::at, return_internal_reference<>()) + * .def("pose3_at", &Values::at, return_internal_reference<>()) + * + * and then they could be accessed from python as + * + * >>> import gtsam + * >>> v = gtsam.nonlinear.Values() + * >>> v.insert(1,gtsam.geometry.Point3()) + * >>> v.insert(2,gtsam.geometry.Rot3()) + * >>> v.insert(3,gtsam.geometry.Pose3()) + * >>> v.point3_at(1) + * >>> v.rot3_at(2) + * >>> v.pose3_at(3) + * + * In fact, I just saw the pythonic way sounds more clear, so I'm sticking with this and + * leaving the comments here for future reference. I'm using the PEP0008 for method naming. + * See: https://www.python.org/dev/peps/pep-0008/#function-and-method-arguments + */ +// template +// const T & ValuesAt( const Values & v, Key j, T /*type*/) +// { +// return v.at(j); +// } + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1); + +void exportValues(){ + + // NOTE: Apparently the class 'Value'' is deprecated, so the commented lines below + // will compile, but are useless in the python wrapper. We need to use specific + // 'at' and 'insert' methods for each type. + // const Value& (Values::*at1)(Key) const = &Values::at; + // void (Values::*insert1)(Key, const Value&) = &Values::insert; + bool (Values::*exists1)(Key) const = &Values::exists; + void (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert; + void (Values::*insert_rot2) (Key, const gtsam::Rot2&) = &Values::insert; + void (Values::*insert_pose2) (Key, const gtsam::Pose2&) = &Values::insert; + void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert; + void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; + void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; + + + class_("Values", init<>()) + .def(init()) + .def("clear", &Values::clear) + .def("dim", &Values::dim) + .def("empty", &Values::empty) + .def("equals", &Values::equals) + .def("erase", &Values::erase) + .def("insert_fixed", &Values::insertFixed) + .def("print", &Values::print, print_overloads(args("s"))) + .def("size", &Values::size) + .def("swap", &Values::swap) + // NOTE: Following commented lines add useless methods on Values + // .def("insert", insert1) + // .def("at", at1, return_value_policy()) + .def("insert", insert_point2) + .def("insert", insert_rot2) + .def("insert", insert_pose2) + .def("insert", insert_point3) + .def("insert", insert_rot3) + .def("insert", insert_pose3) + // NOTE: The following commented lines are another way of specializing the return type. + // See long comment above. + // .def("at", &ValuesAt, return_internal_reference<>()) + // .def("at", &ValuesAt, return_internal_reference<>()) + // .def("at", &ValuesAt, return_internal_reference<>()) + .def("point3_at", &Values::at, return_value_policy()) + .def("rot3_at", &Values::at, return_value_policy()) + .def("pose3_at", &Values::at, return_value_policy()) + .def("exists", exists1) + .def("keys", &Values::keys) + ; +} diff --git a/python/handwritten/slam/BearingFactor.cpp b/python/handwritten/slam/BearingFactor.cpp new file mode 100644 index 000000000..84c67d522 --- /dev/null +++ b/python/handwritten/slam/BearingFactor.cpp @@ -0,0 +1,17 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +template +void exportBearingFactor(const std::string& name){ + class_(name, init<>()) + ; +} \ No newline at end of file diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp new file mode 100644 index 000000000..b6fc552a0 --- /dev/null +++ b/python/handwritten/slam/BetweenFactor.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps BetweenFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/BetweenFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template +// void exportBetweenFactor(const std::string& name){ +// class_(name, init<>()) +// .def(init()) +// ; +// } + +#define BETWEENFACTOR(VALUE) \ + class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#VALUE) \ + .def(init()) \ + .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ +; + +void exportBetweenFactors() +{ + BETWEENFACTOR(Point2) + BETWEENFACTOR(Rot2) + BETWEENFACTOR(Pose2) + BETWEENFACTOR(Point3) + BETWEENFACTOR(Rot3) + BETWEENFACTOR(Pose3) +} diff --git a/python/handwritten/slam/GenericProjectionFactor.cpp b/python/handwritten/slam/GenericProjectionFactor.cpp new file mode 100644 index 000000000..dd932ccd4 --- /dev/null +++ b/python/handwritten/slam/GenericProjectionFactor.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps GenericProjectionFactor for several values to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/ProjectionFactor.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, GenericProjectionFactorCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, GenericProjectionFactorCal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(evaluateError_overloads, GenericProjectionFactorCal3_S2::evaluateError, 2, 4) + +void exportGenericProjectionFactor() +{ + + class_ >("GenericProjectionFactorCal3_S2", init<>()) + .def(init &, optional >()) + .def(init &, bool, bool, optional >()) + .def("print", &GenericProjectionFactorCal3_S2::print, print_overloads(args("s"))) + .def("equals", &GenericProjectionFactorCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("evaluate_error", &GenericProjectionFactorCal3_S2::evaluateError, evaluateError_overloads()) + .def("measured", &GenericProjectionFactorCal3_S2::measured, return_value_policy()) + // TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &' + // .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy()) + .def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality) + .def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality) + ; + +} diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp new file mode 100644 index 000000000..dcb9de8ea --- /dev/null +++ b/python/handwritten/slam/PriorFactor.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PriorFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/PriorFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template< class FACTOR, class VALUE > +// void exportPriorFactor(const std::string& name){ +// class_< FACTOR >(name.c_str(), init<>()) +// .def(init< Key, VALUE&, SharedNoiseModel >()) +// ; +// } + +#define PRIORFACTOR(VALUE) \ + class_< PriorFactor, bases, boost::shared_ptr< PriorFactor > >("PriorFactor"#VALUE) \ + .def(init()) \ + .def("prior", &PriorFactor::prior, return_internal_reference<>()) \ +; + +void exportPriorFactors() +{ + PRIORFACTOR(Point2) + PRIORFACTOR(Rot2) + PRIORFACTOR(Pose2) + PRIORFACTOR(Point3) + PRIORFACTOR(Rot3) + PRIORFACTOR(Pose3) +} \ No newline at end of file diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp new file mode 100644 index 000000000..d7cebe7ad --- /dev/null +++ b/python/handwritten/utils/NumpyEigen.cpp @@ -0,0 +1,54 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief register conversion matrix between numpy and Eigen + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace boost::python; +using namespace gtsam; + +void registerNumpyEigenConversions() +{ + // NOTE: import array should be called only in the cpp defining the module + // import_array(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + +} diff --git a/python/setup.py.in b/python/setup.py.in new file mode 100644 index 000000000..d3b5fcde4 --- /dev/null +++ b/python/setup.py.in @@ -0,0 +1,15 @@ +from distutils.core import setup + +setup(name='gtsam', + version='${GTSAM_VERSION_STRING}', + description='GTSAM Python wrapper', + license = "BSD", + author='Frank Dellaert et. al', + author_email='frank.dellaert@gatech.edu', + maintainer_email='gtsam@lists.gatech.edu', + url='https://collab.cc.gatech.edu/borg/gtsam', + package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, + packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], + #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + )