diff --git a/.gitignore b/.gitignore index 03ce578e7d..1d89cac258 100644 --- a/.gitignore +++ b/.gitignore @@ -8,7 +8,6 @@ /examples/Data/pose3example-rewritten.txt *.txt.user *.txt.user.6d59f0c -/python-build/ *.pydevproject cython/venv cython/gtsam.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ffcb5341f2..3654a99a47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,7 +64,6 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) 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" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) @@ -78,7 +77,7 @@ endif() option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_INSTALL_CYTHON_TOOLBOX "Enable/Disable installation of Cython toolbox" OFF) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON) -set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper or python module for (or Default)") +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper for (or Default)") # Check / set dependent variables for MATLAB wrapper if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP) @@ -98,10 +97,6 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") endif() -if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4) - message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") -endif() - if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS) message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") endif() @@ -423,20 +418,6 @@ 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() - # Cython wrap if (GTSAM_INSTALL_CYTHON_TOOLBOX) set(GTSAM_INSTALL_CYTHON_TOOLBOX 1) @@ -570,29 +551,19 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") -print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ") print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") +print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") 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 "Cython toolbox flags ") print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") -message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") -print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") +if(GTSAM_INSTALL_CYTHON_TOOLBOX) + message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") +endif() message(STATUS "===============================================================") # Print warnings at the end @@ -605,9 +576,6 @@ endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") 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/python/.gitignore b/python/.gitignore deleted file mode 100644 index d16386367f..0000000000 --- a/python/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build/ \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt deleted file mode 100644 index f7ceb62b37..0000000000 --- a/python/CMakeLists.txt +++ /dev/null @@ -1,96 +0,0 @@ -# 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_CURRENT_SOURCE_DIR}/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 deleted file mode 100644 index 396bcea893..0000000000 --- a/python/README.md +++ /dev/null @@ -1,26 +0,0 @@ -Python Wrapper and Packaging -============================ - -# Deprecation Notice - -We are in the process of deprecating the old Python bindings and Cython bindings in favor of the new Pybind11 binding system. - -As such, the examples in this directory are no longer guaranteed to work. Please refer to the [cython examples](https://bitbucket.org/gtborg/gtsam/src/develop/cython/gtsam/examples). - ---- - -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 deleted file mode 100644 index 2d7ac72f70..0000000000 --- a/python/gtsam/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from _gtsampy import * diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py deleted file mode 100644 index 781dae118f..0000000000 --- a/python/gtsam_examples/ImuFactorExample.py +++ /dev/null @@ -1,123 +0,0 @@ -""" -A script validating the ImuFactor inference. -""" - -from __future__ import print_function -import math -import matplotlib.pyplot as plt -import numpy as np - -from mpl_toolkits.mplot3d import Axes3D - -import gtsam -from gtsam_utils import plotPose3 -from PreintegrationExample import PreintegrationExample, POSES_FIG - -# shorthand symbols: -BIAS_KEY = int(gtsam.Symbol('b', 0)) -V = lambda j: int(gtsam.Symbol('v', j)) -X = lambda i: int(gtsam.Symbol('x', i)) - -np.set_printoptions(precision=3, suppress=True) - -class ImuFactorExample(PreintegrationExample): - - def __init__(self): - self.velocity = np.array([2, 0, 0]) - self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - - # Choose one of these twists to change scenario: - zero_twist = (np.zeros(3), np.zeros(3)) - forward_twist = (np.zeros(3), self.velocity) - loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) - sick_twist = (np.array([math.radians(30), -math.radians(30), 0]), self.velocity) - - accBias = np.array([-0.3, 0.1, 0.2]) - gyroBias = np.array([0.1, 0.3, -0.1]) - bias = gtsam.ConstantBias(accBias, gyroBias) - - dt = 1e-2 - super(ImuFactorExample, self).__init__(sick_twist, bias, dt) - - def addPrior(self, i, graph): - state = self.scenario.navState(i) - graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) - graph.push_back(gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise)) - - def run(self): - graph = gtsam.NonlinearFactorGraph() - - # initialize data structure for pre-integrated IMU measurements - pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - - H9 = gtsam.OptionalJacobian9(); - - T = 12 - num_poses = T + 1 # assumes 1 factor per second - initial = gtsam.Values() - initial.insert(BIAS_KEY, self.actualBias) - for i in range(num_poses): - state_i = self.scenario.navState(float(i)) - initial.insert(X(i), state_i.pose()) - initial.insert(V(i), state_i.velocity()) - - # simulate the loop - i = 0 # state index - actual_state_i = self.scenario.navState(0) - for k, t in enumerate(np.arange(0, T, self.dt)): - # get measurements and add them to PIM - measuredOmega = self.runner.measuredAngularVelocity(t) - measuredAcc = self.runner.measuredSpecificForce(t) - pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) - - # Plot IMU many times - if k % 10 == 0: - self.plotImu(t, measuredOmega, measuredAcc) - - # Plot every second - if k % int(1 / self.dt) == 0: - self.plotGroundTruthPose(t) - - # create IMU factor every second - if (k + 1) % int(1 / self.dt) == 0: - factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) - graph.push_back(factor) - if True: - print(factor) - H2 = gtsam.OptionalJacobian96(); - print(pim.predict(actual_state_i, self.actualBias, H9, H2)) - pim.resetIntegration() - actual_state_i = self.scenario.navState(t + self.dt) - i += 1 - - # add priors on beginning and end - self.addPrior(0, graph) - self.addPrior(num_poses - 1, graph) - - # optimize using Levenberg-Marquardt optimization - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("SUMMARY") - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = optimizer.optimize() - - # Calculate and print marginal covariances - marginals = gtsam.Marginals(graph, result) - print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) - for i in range(num_poses): - print("Covariance on pose {}:\n{}\n".format(i, marginals.marginalCovariance(X(i)))) - print("Covariance on vel {}:\n{}\n".format(i, marginals.marginalCovariance(V(i)))) - - # Plot resulting poses - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plotPose3(POSES_FIG, pose_i, 0.1) - i += 1 - print(result.atConstantBias(BIAS_KEY)) - - plt.ioff() - plt.show() - -if __name__ == '__main__': - ImuFactorExample().run() diff --git a/python/gtsam_examples/OdometryExample.py b/python/gtsam_examples/OdometryExample.py deleted file mode 100644 index 0800bab4ee..0000000000 --- a/python/gtsam_examples/OdometryExample.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/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/Pose2SLAMExample.py b/python/gtsam_examples/Pose2SLAMExample.py deleted file mode 100644 index 7bb1296d7d..0000000000 --- a/python/gtsam_examples/Pose2SLAMExample.py +++ /dev/null @@ -1,68 +0,0 @@ -from __future__ import print_function -import gtsam -import math -import numpy as np - -def Vector3(x, y, z): return np.array([x, y, z]) - -# 1. Create a factor graph container and add factors to it -graph = gtsam.NonlinearFactorGraph() - -# 2a. Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorNoise = gtsam.noiseModel.Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) -graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise)) - -# For simplicity, we will use the same noise model for odometry and loop closures -model = gtsam.noiseModel.Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) - -# 2b. Add odometry factors -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model)) -graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), model)) -graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), model)) -graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), model)) - -# 2c. Add the loop closure constraint -# This factor encodes the fact that we have returned to the same pose. In real -# systems, these constraints may be identified in many ways, such as appearance-based -# techniques with camera images. We will use another Between Factor to enforce this constraint: -graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), model)) -graph.print("\nFactor Graph:\n") # print - -# 3. Create the data structure to hold the initialEstimate estimate to the -# solution. For illustrative purposes, these have been deliberately set to incorrect values -initialEstimate = gtsam.Values() -initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) -initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) -initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) -initialEstimate.print("\nInitial Estimate:\n") # print - -# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer -# The optimizer accepts an optional set of configuration parameters, -# controlling things like convergence criteria, the type of linear -# system solver to use, and the amount of information displayed during -# optimization. We will set a few parameters as a demonstration. -parameters = gtsam.GaussNewtonParams() - -# Stop iterating once the change in error between steps is less than this value -parameters.relativeErrorTol = 1e-5 -# Do not perform more than N iteration steps -parameters.maxIterations = 100 -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initialEstimate, parameters) -# ... and optimize -result = optimizer.optimize() -result.print("Final Result:\n") - -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -print("x1 covariance:\n", marginals.marginalCovariance(1)) -print("x2 covariance:\n", marginals.marginalCovariance(2)) -print("x3 covariance:\n", marginals.marginalCovariance(3)) -print("x4 covariance:\n", marginals.marginalCovariance(4)) -print("x5 covariance:\n", marginals.marginalCovariance(5)) - - diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py deleted file mode 100644 index b441ffecbe..0000000000 --- a/python/gtsam_examples/PreintegrationExample.py +++ /dev/null @@ -1,129 +0,0 @@ -""" -A script validating the Preintegration of IMU measurements -""" - -import math -import matplotlib.pyplot as plt -import numpy as np - -from mpl_toolkits.mplot3d import Axes3D - -import gtsam -from gtsam_utils import plotPose3 - -IMU_FIG = 1 -POSES_FIG = 2 - -class PreintegrationExample(object): - - @staticmethod - def defaultParams(g): - """Create default parameters with Z *up* and realistic noise parameters""" - params = gtsam.PreintegrationParams.MakeSharedU(g) - kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW - kAccelSigma = 0.1 / 60 # 10 cm VRW - params.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float) - params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float) - params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) - return params - - def __init__(self, twist=None, bias=None, dt=1e-2): - """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" - - # setup interactive plotting - plt.ion() - - # Setup loop as default scenario - if twist is not None: - (W, V) = twist - else: - # default = loop with forward velocity 2m/s, while pitching up - # with angular velocity 30 degree/sec (negative in FLU) - W = np.array([0, -math.radians(30), 0]) - V = np.array([2, 0, 0]) - - self.scenario = gtsam.ConstantTwistScenario(W, V) - self.dt = dt - - self.maxDim = 5 - self.labels = list('xyz') - self.colors = list('rgb') - - # Create runner - self.g = 10 # simple gravity constant - self.params = self.defaultParams(self.g) - ptr = gtsam.ScenarioPointer(self.scenario) - - if bias is not None: - self.actualBias = bias - else: - accBias = np.array([0, 0.1, 0]) - gyroBias = np.array([0, 0, 0]) - self.actualBias = gtsam.ConstantBias(accBias, gyroBias) - - self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias) - - def plotImu(self, t, measuredOmega, measuredAcc): - plt.figure(IMU_FIG) - - # plot angular velocity - omega_b = self.scenario.omega_b(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 1) - plt.scatter(t, omega_b[i], color='k', marker='.') - plt.scatter(t, measuredOmega[i], color=color, marker='.') - plt.xlabel('angular velocity ' + label) - - # plot acceleration in nav - acceleration_n = self.scenario.acceleration_n(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 4) - plt.scatter(t, acceleration_n[i], color=color, marker='.') - plt.xlabel('acceleration in nav ' + label) - - # plot acceleration in body - acceleration_b = self.scenario.acceleration_b(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 7) - plt.scatter(t, acceleration_b[i], color=color, marker='.') - plt.xlabel('acceleration in body ' + label) - - # plot actual specific force, as well as corrupted - actual = self.runner.actualSpecificForce(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(4, 3, i + 10) - plt.scatter(t, actual[i], color='k', marker='.') - plt.scatter(t, measuredAcc[i], color=color, marker='.') - plt.xlabel('specific force ' + label) - - def plotGroundTruthPose(self, t): - # plot ground truth pose, as well as prediction from integrated IMU measurements - actualPose = self.scenario.pose(t) - plotPose3(POSES_FIG, actualPose, 0.3) - t = actualPose.translation() - self.maxDim = max([abs(t[0]), abs(t[1]), abs(t[2]), self.maxDim]) - ax = plt.gca() - ax.set_xlim3d(-self.maxDim, self.maxDim) - ax.set_ylim3d(-self.maxDim, self.maxDim) - ax.set_zlim3d(-self.maxDim, self.maxDim) - - plt.pause(0.01) - - def run(self): - # simulate the loop - T = 12 - for i, t in enumerate(np.arange(0, T, self.dt)): - measuredOmega = self.runner.measuredAngularVelocity(t) - measuredAcc = self.runner.measuredSpecificForce(t) - if i % 25 == 0: - self.plotImu(t, measuredOmega, measuredAcc) - self.plotGroundTruthPose(t) - pim = self.runner.integrate(t, self.actualBias, True) - predictedNavState = self.runner.predict(pim, self.actualBias) - plotPose3(POSES_FIG, predictedNavState.pose(), 0.1) - - plt.ioff() - plt.show() - -if __name__ == '__main__': - PreintegrationExample().run() diff --git a/python/gtsam_examples/SFMdata.py b/python/gtsam_examples/SFMdata.py deleted file mode 100644 index 21a4382269..0000000000 --- a/python/gtsam_examples/SFMdata.py +++ /dev/null @@ -1,32 +0,0 @@ - - # 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 deleted file mode 100644 index 9dafa13e73..0000000000 --- a/python/gtsam_examples/VisualISAM2Example.py +++ /dev/null @@ -1,132 +0,0 @@ -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 - -# shorthand symbols: -X = lambda i: int(gtsam.Symbol('x', i)) -L = lambda j: int(gtsam.Symbol('l', j)) - -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 - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - gtsam_utils.plotPose3(fignum, pose_i, 10) - i += 1 - - # draw - ax.set_xlim3d(-40, 40) - ax.set_ylim3d(-40, 40) - ax.set_zlim3d(-40, 40) - plt.pause(1) - -def visual_ISAM2_example(): - plt.ion() - - # Define the camera calibration parameters - K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) - - # Define the camera observation noise model - 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, X(i), L(j), K)) - - # Add an initial guess for the current pose - # Intentionally initialize the variables off from the ground truth - initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - - # If this is the first iteration, add a prior on the first pose to set the coordinate frame - # and a prior on the first landmark to set the scale - # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before - # adding it to iSAM. - if(i == 0): - # Add a prior on pose x0 - 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(X(0), poses[0], poseNoise)) - - # Add a prior on landmark l0 - pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - graph.push_back(gtsam.PriorFactorPoint3(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(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(X(j), ":", currentEstimate.atPose3(X(j))) - - for j in range(len(points)): - print(L(j), ":", currentEstimate.atPoint3(L(j))) - - visual_ISAM2_plot(poses, points, currentEstimate) - - # Clear the factor graph and values for the next iteration - graph.resize(0) - initialEstimate.clear() - - plt.ioff() - plt.show() - -if __name__ == '__main__': - visual_ISAM2_example() diff --git a/python/gtsam_examples/__init__.py b/python/gtsam_examples/__init__.py deleted file mode 100644 index f9d032d544..0000000000 --- a/python/gtsam_examples/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from . import SFMdata -from . import VisualISAM2Example diff --git a/python/gtsam_tests/__init__.py b/python/gtsam_tests/__init__.py deleted file mode 100644 index 8b13789179..0000000000 --- a/python/gtsam_tests/__init__.py +++ /dev/null @@ -1 +0,0 @@ - diff --git a/python/gtsam_tests/testPoint2.py b/python/gtsam_tests/testPoint2.py deleted file mode 100644 index 80a6f6cbfa..0000000000 --- a/python/gtsam_tests/testPoint2.py +++ /dev/null @@ -1,13 +0,0 @@ -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_tests/testScenario.py b/python/gtsam_tests/testScenario.py deleted file mode 100644 index e781212415..0000000000 --- a/python/gtsam_tests/testScenario.py +++ /dev/null @@ -1,32 +0,0 @@ -import math -import unittest -import numpy as np - -import gtsam - -class TestScenario(unittest.TestCase): - def setUp(self): - pass - - def test_loop(self): - # Forward velocity 2m/s - # Pitch up with angular velocity 6 degree/sec (negative in FLU) - v = 2 - w = math.radians(6) - W = np.array([0, -w, 0]) - V = np.array([v, 0, 0]) - scenario = gtsam.ConstantTwistScenario(W, V) - - T = 30 - np.testing.assert_almost_equal(W, scenario.omega_b(T)) - np.testing.assert_almost_equal(V, scenario.velocity_b(T)) - np.testing.assert_almost_equal(np.cross(W, V), scenario.acceleration_b(T)) - - # R = v/w, so test if loop crests at 2*R - R = v / w - T30 = scenario.pose(T) - np.testing.assert_almost_equal(np.array([-math.pi, 0, -math.pi]), T30.rotation().xyz()) - self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation())) - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam_tests/testScenarioRunner.py b/python/gtsam_tests/testScenarioRunner.py deleted file mode 100644 index 2e1b472024..0000000000 --- a/python/gtsam_tests/testScenarioRunner.py +++ /dev/null @@ -1,30 +0,0 @@ -import math -import unittest -import numpy as np - -import gtsam - -class TestScenarioRunner(unittest.TestCase): - def setUp(self): - self.g = 10 # simple gravity constant - - def test_loop_runner(self): - # Forward velocity 2m/s - # Pitch up with angular velocity 6 degree/sec (negative in FLU) - v = 2 - w = math.radians(6) - W = np.array([0, -w, 0]) - V = np.array([v, 0, 0]) - scenario = gtsam.ConstantTwistScenario(W, V) - - dt = 0.1 - params = gtsam.PreintegrationParams.MakeSharedU(self.g) - runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(scenario), params, dt) - - # Test specific force at time 0: a is pointing up - t = 0.0 - a = w * v - np.testing.assert_almost_equal(np.array([0, 0, a + self.g]), runner.actualSpecificForce(t)) - -if __name__ == '__main__': - unittest.main() diff --git a/python/gtsam_utils/__init__.py b/python/gtsam_utils/__init__.py deleted file mode 100644 index 56c55aa949..0000000000 --- a/python/gtsam_utils/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .plot import * diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py deleted file mode 100644 index 8863f427e4..0000000000 --- a/python/gtsam_utils/plot.py +++ /dev/null @@ -1,62 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - -def plotPoint3OnAxes(ax, point, linespec): - ax.plot([point.x()], [point.y()], [point.z()], linespec) - -def plotPoint3(fignum, point, linespec): - fig = plt.figure(fignum) - ax = fig.gca(projection='3d') - plotPoint3OnAxes(ax, point, 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.atPoint3(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 plotPose3OnAxes(ax, pose, axisLength=0.1): - # get rotation and translation (center) - gRp = pose.rotation().matrix() # rotation from pose to global - C = pose.translation() - - # 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 - -def plotPose3(fignum, pose, axisLength=0.1): - # get figure object - fig = plt.figure(fignum) - ax = fig.gca(projection='3d') - plotPose3OnAxes(ax, pose, axisLength) diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt deleted file mode 100644 index 689354b4ef..0000000000 --- a/python/handwritten/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -# 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}) -string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) -set_target_properties(gtsam_python PROPERTIES - OUTPUT_NAME _gtsampy - PREFIX "" - ${build_type_toupper}_POSTFIX "" - SKIP_BUILD_RPATH TRUE - CLEAN_DIRECT_OUTPUT 1 -) - -target_include_directories(gtsam_python SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR}) -target_include_directories(gtsam_python SYSTEM PUBLIC ${NUMPY_INCLUDE_DIRS}) -target_include_directories(gtsam_python SYSTEM PUBLIC ${PYTHON_INCLUDE_DIRS}) -target_include_directories(gtsam_python SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) -target_include_directories(gtsam_python SYSTEM PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include/) - -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) -set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_gtsampy.so) -add_custom_command( - OUTPUT ${output_path} - DEPENDS gtsam_python - COMMAND ${CMAKE_COMMAND} -E copy $ ${output_path} - COMMENT "Copying extension module to python/gtsam/_gtsampy.so" -) -add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path}) \ No newline at end of file diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp deleted file mode 100644 index 3794886319..0000000000 --- a/python/handwritten/base/FastVector.cpp +++ /dev/null @@ -1,37 +0,0 @@ - /* ---------------------------------------------------------------------------- - - * 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/common.h b/python/handwritten/common.h deleted file mode 100644 index 2690d52fc7..0000000000 --- a/python/handwritten/common.h +++ /dev/null @@ -1,31 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 common macros used by handwritten exports of the python module - * @author Ellon Paiva Mendes (LAAS-CNRS) - **/ - -#pragma once - - /* Fix to avoid registration warnings */ -// Solution taken from https://github.com/BVLC/caffe/pull/4069/commits/673e8cfc0b8f05f9fa3ebbad7cc6202822e5d9c5 -#define REGISTER_SHARED_PTR_TO_PYTHON(PTR) do { \ - const boost::python::type_info info = \ - boost::python::type_id >(); \ - const boost::python::converter::registration* reg = \ - boost::python::converter::registry::query(info); \ - if (reg == NULL) { \ - boost::python::register_ptr_to_python >(); \ - } else if ((*reg).m_to_python == NULL) { \ - boost::python::register_ptr_to_python >(); \ - } \ -} while (0) diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp deleted file mode 100644 index 8fa7e0fdd0..0000000000 --- a/python/handwritten/exportgtsam.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - -// base -void exportFastVectors(); - -// geometry -void exportPoint2(); -void exportPoint3(); -void exportRot2(); -void exportRot3(); -void exportPose2(); -void exportPose3(); -void exportPinholeBaseK(); -void exportPinholeCamera(); -void exportCal3_S2(); -void export_geometry(); - -// inference -void exportSymbol(); - -// linear -void exportNoiseModels(); - -// nonlinear -void exportValues(); -void exportNonlinearFactor(); -void exportNonlinearFactorGraph(); -void exportLevenbergMarquardtOptimizer(); -void exportISAM2(); - -// slam -void exportPriorFactors(); -void exportBetweenFactors(); -void exportGenericProjectionFactor(); -void export_slam(); - -// navigation -void exportImuFactor(); -void exportScenario(); - - -// Utils (or Python wrapper specific functions) -void registerNumpyEigenConversions(); - -//-----------------------------------// - -BOOST_PYTHON_MODULE(_gtsampy){ - - // 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(); - export_geometry(); - - exportSymbol(); - - exportNoiseModels(); - - exportValues(); - exportNonlinearFactor(); - exportNonlinearFactorGraph(); - exportLevenbergMarquardtOptimizer(); - exportISAM2(); - - exportPriorFactors(); - exportBetweenFactors(); - exportGenericProjectionFactor(); - export_slam(); - - exportImuFactor(); - exportScenario(); -} diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp deleted file mode 100644 index 1d59b1780e..0000000000 --- a/python/handwritten/geometry/Cal3_S2.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - -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(args("fov","w","h"))) - .def(init()) - .def(repr(self)) - .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()) -; -register_ptr_to_python< boost::shared_ptr >(); - -} diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp deleted file mode 100644 index 4153ecf069..0000000000 --- a/python/handwritten/geometry/PinholeBaseK.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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, 1, 4) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3) - -// Function pointers to disambiguate project() calls -Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; -Point2 (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; - -// function pointers to disambiguate 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; - -// wrap projectSafe in a function that returns None or a tuple -// TODO(frank): find out how to return an ndarray instead -object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) { - auto result = camera.projectSafe(p); - if (result.second) - return make_tuple(result.first.x(), result.first.y()); - else - return object(); -} - -void exportPinholeBaseK() { - class_("PinholeBaseKCal3_S2", no_init) - .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), - return_value_policy()) - .def("project_safe", make_function(project_safe)) - .def("project", project1, project_overloads()) - .def("project", project2, 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()); -} diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp deleted file mode 100644 index 97f6e09c6f..0000000000 --- a/python/handwritten/geometry/PinholeCamera.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 deleted file mode 100644 index e7590976f9..0000000000 --- a/python/handwritten/geometry/Point2.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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; - -#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS -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) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(distance_overloads, Point2::distance, 1, 3) -#endif - -void exportPoint2(){ - -#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS - class_("Point2", init<>()) - .def(init()) - .def(init()) - .def("identity", &Point2::identity) - .def("distance", &Point2::distance, distance_overloads(args("q","H1","H2"))) - .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, return_value_policy()) - .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) - ; -#endif -} diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp deleted file mode 100644 index 7935d6b37b..0000000000 --- a/python/handwritten/geometry/Point3.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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; - -#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1) -#endif - -void exportPoint3(){ - -#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS -class_("Point3") - .def(init<>()) - .def(init()) - .def(init()) - .def("vector", &Point3::vector, return_value_policy()) - .def("x", &Point3::x) - .def("y", &Point3::y) - .def("z", &Point3::z) - .def("print", &Point3::print, print_overloads(args("s"))) - .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) - .def("distance", &Point3::distance) - .def("cross", &Point3::cross) - .def("dot", &Point3::dot) - .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) - .def("normalized", &Point3::normalized) - .def("identity", &Point3::identity) - .staticmethod("identity") - .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); -#endif - -class_("Point3Pair", init()) - .def_readwrite("first", &Point3Pair::first) - .def_readwrite("second", &Point3Pair::second); -} diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp deleted file mode 100644 index aaf17f8125..0000000000 --- a/python/handwritten/geometry/Pose2.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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::transformTo, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transformFrom, 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("transformTo", &Pose2::transformTo, - transform_to_overloads(args("point", "H1", "H2"))) - .def("transformFrom", &Pose2::transformFrom, - 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 deleted file mode 100644 index cbddab0a95..0000000000 --- a/python/handwritten/geometry/Pose3.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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::transformTo, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transformFrom, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1) -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 transformTo() calls - Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transformTo; - Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transformTo; - // 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; - // function pointers to desambiguate bearing() calls - Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const = &Pose3::bearing; - Unit3 (Pose3::*bearing2)(const Pose3 &, OptionalJacobian<2,6>, OptionalJacobian<2,6>) const = &Pose3::bearing; - - 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("matrix", &Pose3::matrix) - .def("transformFrom", &Pose3::transformFrom, - transform_from_overloads(args("point", "H1", "H2"))) - .def("transformTo", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) - .def("transformTo", transform_to2) - .def("x", &Pose3::x) - .def("y", &Pose3::y) - .def("z", &Pose3::z) - .def("rotation", &Pose3::rotation, - rotation_overloads()[return_value_policy()]) - .def("translation", &Pose3::translation, - translation_overloads()[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", bearing1, bearing_overloads()) - .def("bearing", bearing2, bearing_overloads()); -} diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp deleted file mode 100644 index 961ad970fb..0000000000 --- a/python/handwritten/geometry/Rot2.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 deleted file mode 100644 index 440559e3e2..0000000000 --- a/python/handwritten/geometry/Rot3.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 gtsam::Point3&, double) = &Rot3::AxisAngle; -gtsam::Rot3 (*AxisAngle_1)(const gtsam::Unit3&, 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(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("Expmap", &Rot3::Expmap) - .staticmethod("Expmap") - .def("ExpmapDerivative", &Rot3::ExpmapDerivative) - .staticmethod("ExpmapDerivative") - .def("Logmap", &Rot3::Logmap) - .staticmethod("Logmap") - .def("LogmapDerivative", &Rot3::LogmapDerivative) - .staticmethod("LogmapDerivative") - .def("AxisAngle", AxisAngle_0) - .def("AxisAngle", AxisAngle_1) - .staticmethod("AxisAngle") - .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("Ypr", &Rot3::Ypr) - .staticmethod("Ypr") - .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__ - ; - -} diff --git a/python/handwritten/geometry/export_geometry.cpp b/python/handwritten/geometry/export_geometry.cpp deleted file mode 100644 index 8c04f23dc4..0000000000 --- a/python/handwritten/geometry/export_geometry.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file export_geometry - * @brief wraps geometry classes - * @author Ellon Paiva Mendes (LAAS-CNRS) - * @author Frank Dellaert - **/ - -#include - -#define NO_IMPORT_ARRAY -#include - -#include - -using namespace boost::python; -using namespace gtsam; -using namespace std; - -void export_geometry() { - class_("Unit3") - .def(init<>()) - .def(init()) - .def(init()); -} diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp deleted file mode 100644 index 0ec3445b86..0000000000 --- a/python/handwritten/inference/Symbol.cpp +++ /dev/null @@ -1,83 +0,0 @@ - /* ---------------------------------------------------------------------------- - - * 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 deleted file mode 100644 index c4c46fe7fa..0000000000 --- a/python/handwritten/linear/NoiseModel.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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" - -#include "python/handwritten/common.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)) - ; - register_ptr_to_python< boost::shared_ptr >(); - - // 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") - ; - REGISTER_SHARED_PTR_TO_PYTHON(Gaussian); - - 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") - ; - REGISTER_SHARED_PTR_TO_PYTHON(Diagonal); - - 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") - ; - REGISTER_SHARED_PTR_TO_PYTHON(Isotropic); - - class_, bases >("Unit", no_init) - .def("Create",&Unit::Create) - .staticmethod("Create") - ; - - REGISTER_SHARED_PTR_TO_PYTHON(Unit); -} diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp deleted file mode 100644 index 0cf3062b5d..0000000000 --- a/python/handwritten/navigation/ImuFactor.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ConstantTwistScenario class to python - * @author Frank Dellaert - **/ - -#include - -#define NO_IMPORT_ARRAY -#include - -#include "gtsam/navigation/ImuFactor.h" -#include "gtsam/navigation/GPSFactor.h" - -#include "python/handwritten/common.h" - -using namespace boost::python; -using namespace gtsam; - -typedef gtsam::OptionalJacobian<3, 9> OptionalJacobian39; -typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96; -typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9; - -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PreintegratedImuMeasurements::equals, 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationBase::predict, 2, 4) - -void exportImuFactor() { - class_("OptionalJacobian39", init<>()); - class_("OptionalJacobian96", init<>()); - class_("OptionalJacobian9", init<>()); - - class_("NavState", init<>()) - .def(init()) - // TODO(frank): overload with jacobians - .def("print", &NavState::print, print_overloads()) - .def("attitude", &NavState::attitude, - attitude_overloads()[return_value_policy()]) - .def("position", &NavState::position, - position_overloads()[return_value_policy()]) - .def("velocity", &NavState::velocity, - velocity_overloads()[return_value_policy()]) - .def(repr(self)) - .def("pose", &NavState::pose); - - class_("ConstantBias", init<>()) - .def(init()) - .def(repr(self)); - - class_>( - "PreintegrationParams", init()) - .def_readwrite("gyroscopeCovariance", - &PreintegrationParams::gyroscopeCovariance) - .def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis) - .def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor) - .def_readwrite("accelerometerCovariance", - &PreintegrationParams::accelerometerCovariance) - .def_readwrite("integrationCovariance", - &PreintegrationParams::integrationCovariance) - .def_readwrite("use2ndOrderCoriolis", - &PreintegrationParams::use2ndOrderCoriolis) - .def_readwrite("n_gravity", &PreintegrationParams::n_gravity) - - .def("MakeSharedD", &PreintegrationParams::MakeSharedD) - .staticmethod("MakeSharedD") - .def("MakeSharedU", &PreintegrationParams::MakeSharedU) - .staticmethod("MakeSharedU"); - - // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html - REGISTER_SHARED_PTR_TO_PYTHON(PreintegrationParams); - - class_( -#ifdef GTSAM_TANGENT_PREINTEGRATION - "TangentPreintegration", -#else - "ManifoldPreintegration", -#endif - init&, const imuBias::ConstantBias&>()) - .def("predict", &PreintegrationType::predict, predict_overloads()) - .def("computeError", &PreintegrationType::computeError) - .def("resetIntegration", &PreintegrationType::resetIntegration) - .def("deltaTij", &PreintegrationType::deltaTij); - - class_>( - "PreintegratedImuMeasurements", - init&, const imuBias::ConstantBias&>()) - .def(repr(self)) - .def("equals", &PreintegratedImuMeasurements::equals, equals_overloads(args("other", "tol"))) - .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement) - .def("integrateMeasurements", &PreintegratedImuMeasurements::integrateMeasurements) - .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov); - - class_, boost::shared_ptr>("ImuFactor") - .def("error", &ImuFactor::error) - .def(init()) - .def(repr(self)); - REGISTER_SHARED_PTR_TO_PYTHON(ImuFactor); - - class_, boost::shared_ptr>("ImuFactor2") - .def("error", &ImuFactor2::error) - .def(init()) - .def(repr(self)); - REGISTER_SHARED_PTR_TO_PYTHON(ImuFactor2); - - class_, boost::shared_ptr>("GPSFactor") - .def("error", &GPSFactor::error) - .def(init()); - REGISTER_SHARED_PTR_TO_PYTHON(GPSFactor); - - class_, boost::shared_ptr>("GPSFactor2") - .def("error", &GPSFactor2::error) - .def(init()); - REGISTER_SHARED_PTR_TO_PYTHON(GPSFactor2); -} diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp deleted file mode 100644 index e11f04a577..0000000000 --- a/python/handwritten/navigation/Scenario.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ConstantTwistScenario class to python - * @author Frank Dellaert - **/ - -#include - -#define NO_IMPORT_ARRAY -#include - -#include "gtsam/navigation/ScenarioRunner.h" - -using namespace boost::python; -using namespace gtsam; - -// Create const Scenario pointer from ConstantTwistScenario -static const Scenario* ScenarioPointer(const ConstantTwistScenario& scenario) { - return static_cast(&scenario); -} - -void exportScenario() { - // NOTE(frank): Abstract classes need boost::noncopyable - class_("Scenario", no_init); - - // TODO(frank): figure out how to do inheritance - class_("ConstantTwistScenario", - init()) - .def("pose", &Scenario::pose) - .def("omega_b", &Scenario::omega_b) - .def("velocity_n", &Scenario::velocity_n) - .def("acceleration_n", &Scenario::acceleration_n) - .def("navState", &Scenario::navState) - .def("rotation", &Scenario::rotation) - .def("velocity_b", &Scenario::velocity_b) - .def("acceleration_b", &Scenario::acceleration_b); - - // NOTE(frank): https://wiki.python.org/moin/boost.python/CallPolicy - def("ScenarioPointer", &ScenarioPointer, - return_value_policy()); - - class_( - "ScenarioRunner", - init&, - double, const imuBias::ConstantBias&>()) - .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce) - .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity) - .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce) - .def("imuSampleTime", &ScenarioRunner::imuSampleTime, - return_value_policy()) - .def("integrate", &ScenarioRunner::integrate) - .def("predict", &ScenarioRunner::predict) - .def("estimateCovariance", &ScenarioRunner::estimateCovariance); -} diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp deleted file mode 100644 index bd908ce8fa..0000000000 --- a/python/handwritten/nonlinear/ISAM2.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 deleted file mode 100644 index 58e1d116a7..0000000000 --- a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include - -#define NO_IMPORT_ARRAY -#include - -#include -#include -#include - -using namespace boost::python; -using namespace gtsam; - -void exportLevenbergMarquardtOptimizer() { - class_("GaussNewtonParams", init<>()) - .def_readwrite("maxIterations", &GaussNewtonParams::maxIterations) - .def_readwrite("relativeErrorTol", &GaussNewtonParams::relativeErrorTol); - - class_( - "GaussNewtonOptimizer", - init()) - .def("optimize", &GaussNewtonOptimizer::optimize, - return_value_policy()); - - 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()); - - class_("Marginals", init()) - .def("marginalCovariance", &Marginals::marginalCovariance); -} diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp deleted file mode 100644 index 359ac48f3d..0000000000 --- a/python/handwritten/nonlinear/NonlinearFactor.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 NonlinearFactorWrap : 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); - } -}; - -// Similarly for NoiseModelFactor: -struct NoiseModelFactorWrap : NoiseModelFactor, wrapper { - // NOTE(frank): Add all these again as I can't figure out how to derive - 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); - } - Vector unwhitenedError(const Values& x, - boost::optional&> H = boost::none) const { - return this->get_override("unwhitenedError")(x, H); - } -}; - -void exportNonlinearFactor() { - class_("NonlinearFactor") - .def("error", pure_virtual(&NonlinearFactor::error)) - .def("dim", pure_virtual(&NonlinearFactor::dim)) - .def("linearize", pure_virtual(&NonlinearFactor::linearize)); - register_ptr_to_python >(); - - class_("NoiseModelFactor") - .def("error", pure_virtual(&NoiseModelFactor::error)) - .def("dim", pure_virtual(&NoiseModelFactor::dim)) - .def("linearize", pure_virtual(&NoiseModelFactor::linearize)) - .def("unwhitenedError", pure_virtual(&NoiseModelFactor::unwhitenedError)); - register_ptr_to_python >(); -} diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp deleted file mode 100644 index 6898133def..0000000000 --- a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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); - -boost::shared_ptr getNonlinearFactor( - const NonlinearFactorGraph& graph, size_t idx) { - auto p = boost::dynamic_pointer_cast(graph.at(idx)); - if (!p) throw std::runtime_error("No NonlinearFactor at requested index"); - return p; -}; - -void exportNonlinearFactorGraph(){ - - typedef NonlinearFactorGraph::sharedFactor sharedFactor; - - void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; - void (NonlinearFactorGraph::*push_back2)(const NonlinearFactorGraph&) = &NonlinearFactorGraph::push_back; - void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; - - class_("NonlinearFactorGraph", init<>()) - .def("size",&NonlinearFactorGraph::size) - .def("push_back", push_back1) - .def("push_back", push_back2) - .def("add", add1) - .def("error", &NonlinearFactorGraph::error) - .def("resize", &NonlinearFactorGraph::resize) - .def("empty", &NonlinearFactorGraph::empty) - .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) - .def("clone", &NonlinearFactorGraph::clone) - ; - - def("getNonlinearFactor", getNonlinearFactor); - -} diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp deleted file mode 100644 index 9e4aad83f5..0000000000 --- a/python/handwritten/nonlinear/Values.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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" -#include "gtsam/navigation/ImuBias.h" -#include "gtsam/navigation/NavState.h" - -using namespace boost::python; -using namespace gtsam; - -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1); - -void exportValues(){ - - typedef imuBias::ConstantBias Bias; - - 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; - void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; - void (Values::*insert_navstate) (Key, const NavState&) = &Values::insert; - void (Values::*insert_vector) (Key, const gtsam::Vector&) = &Values::insert; - void (Values::*insert_vector2) (Key, const gtsam::Vector2&) = &Values::insert; - void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &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("print", &Values::print, print_overloads(args("s"))) - .def("size", &Values::size) - .def("swap", &Values::swap) - .def("insert", insert_point2) - .def("insert", insert_rot2) - .def("insert", insert_pose2) - .def("insert", insert_point3) - .def("insert", insert_rot3) - .def("insert", insert_pose3) - .def("insert", insert_bias) - .def("insert", insert_navstate) - .def("insert", insert_vector) - .def("insert", insert_vector2) - .def("insert", insert_vector3) - .def("atPoint2", &Values::at) - .def("atRot2", &Values::at) - .def("atPose2", &Values::at) - .def("atPoint3", &Values::at) - .def("atRot3", &Values::at) - .def("atPose3", &Values::at) - .def("atConstantBias", &Values::at) - .def("atNavState", &Values::at) - .def("atVector", &Values::at) - .def("atVector2", &Values::at) - .def("atVector3", &Values::at) - .def("exists", exists1) - .def("keys", &Values::keys) - ; -} diff --git a/python/handwritten/slam/BearingFactor.cpp b/python/handwritten/slam/BearingFactor.cpp deleted file mode 100644 index 2d4688bde4..0000000000 --- a/python/handwritten/slam/BearingFactor.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#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<>()); -} diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp deleted file mode 100644 index 0e0949fb54..0000000000 --- a/python/handwritten/slam/BetweenFactor.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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(T) \ - class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#T) \ - .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 deleted file mode 100644 index 1aa66b8f34..0000000000 --- a/python/handwritten/slam/GenericProjectionFactor.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 deleted file mode 100644 index ea917d2fa2..0000000000 --- a/python/handwritten/slam/PriorFactor.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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" -#include "gtsam/navigation/NavState.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) - PRIORFACTOR(Vector3) - PRIORFACTOR(NavState) -} diff --git a/python/handwritten/slam/export_slam.cpp b/python/handwritten/slam/export_slam.cpp deleted file mode 100644 index 9431af7aa2..0000000000 --- a/python/handwritten/slam/export_slam.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file export_slam - * @brief wraps slam classes - * @author Ellon Paiva Mendes (LAAS-CNRS) - * @author Frank Dellaert - **/ - -#include - -#define NO_IMPORT_ARRAY -#include - -#include - -using namespace boost::python; -using namespace gtsam; -using namespace std; - -void export_slam() { - class_>( - "RotateDirectionsFactor", - init()) - .def("Initialize", &RotateDirectionsFactor::Initialize) - .staticmethod("Initialize"); -} diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp deleted file mode 100644 index 3d3a15727f..0000000000 --- a/python/handwritten/utils/NumpyEigen.cpp +++ /dev/null @@ -1,54 +0,0 @@ - /* ---------------------------------------------------------------------------- - - * 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/include/numpy_eigen/NumpyEigenConverter.hpp b/python/include/numpy_eigen/NumpyEigenConverter.hpp deleted file mode 100755 index 4cf16a974d..0000000000 --- a/python/include/numpy_eigen/NumpyEigenConverter.hpp +++ /dev/null @@ -1,334 +0,0 @@ -/** - * @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/python/include/numpy_eigen/README.md b/python/include/numpy_eigen/README.md deleted file mode 100644 index bd5a3f44d2..0000000000 --- a/python/include/numpy_eigen/README.md +++ /dev/null @@ -1,6 +0,0 @@ -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/python/include/numpy_eigen/boost_python_headers.hpp b/python/include/numpy_eigen/boost_python_headers.hpp deleted file mode 100755 index 5499d2917e..0000000000 --- a/python/include/numpy_eigen/boost_python_headers.hpp +++ /dev/null @@ -1,250 +0,0 @@ -/** - * @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/python/include/numpy_eigen/copy_routines.hpp b/python/include/numpy_eigen/copy_routines.hpp deleted file mode 100755 index b112a7426c..0000000000 --- a/python/include/numpy_eigen/copy_routines.hpp +++ /dev/null @@ -1,149 +0,0 @@ -#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/python/include/numpy_eigen/type_traits.hpp b/python/include/numpy_eigen/type_traits.hpp deleted file mode 100755 index 97e4bb5a04..0000000000 --- a/python/include/numpy_eigen/type_traits.hpp +++ /dev/null @@ -1,191 +0,0 @@ -#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/setup.py.in b/python/setup.py.in deleted file mode 100644 index 8b2de73524..0000000000 --- a/python/setup.py.in +++ /dev/null @@ -1,15 +0,0 @@ -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/_gtsampy.so'])], # location of .so file relative to setup.py - )