Merge pull request #12 from borglab/fix/457_remove_old_python
Fix/457 remove old pythonrelease/4.3a0
Frank Dellaert
GitHub
commit
f5e427a7b2
|
|
@ -8,7 +8,6 @@
|
|||
/examples/Data/pose3example-rewritten.txt
|
||||
*.txt.user
|
||||
*.txt.user.6d59f0c
|
||||
/python-build/
|
||||
*.pydevproject
|
||||
cython/venv
|
||||
cython/gtsam.cpp
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -1 +0,0 @@
|
|||
build/
|
||||
|
|
@ -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()
|
||||
|
|
@ -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.
|
||||
|
||||
|
|
@ -1 +0,0 @@
|
|||
from _gtsampy import *
|
||||
|
|
@ -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()
|
||||
|
|
@ -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")
|
||||
|
|
@ -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))
|
||||
|
||||
|
||||
|
|
@ -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()
|
||||
|
|
@ -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
|
||||
|
|
@ -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()
|
||||
|
|
@ -1,2 +0,0 @@
|
|||
from . import SFMdata
|
||||
from . import VisualISAM2Example
|
||||
|
|
@ -1 +0,0 @@
|
|||
|
||||
|
|
@ -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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -1 +0,0 @@
|
|||
from .plot import *
|
||||
|
|
@ -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)
|
||||
|
|
@ -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 $<TARGET_FILE:gtsam_python> ${output_path}
|
||||
COMMENT "Copying extension module to python/gtsam/_gtsampy.so"
|
||||
)
|
||||
add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path})
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include "gtsam/base/FastVector.h"
|
||||
#include "gtsam/base/types.h" // for Key definition
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
|
||||
void exportFastVectors(){
|
||||
|
||||
typedef FastVector<Key> KeyVector;
|
||||
|
||||
class_<KeyVector>("KeyVector")
|
||||
.def(vector_indexing_suite<KeyVector>())
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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<boost::shared_ptr<PTR > >(); \
|
||||
const boost::python::converter::registration* reg = \
|
||||
boost::python::converter::registry::query(info); \
|
||||
if (reg == NULL) { \
|
||||
boost::python::register_ptr_to_python<boost::shared_ptr<PTR > >(); \
|
||||
} else if ((*reg).m_to_python == NULL) { \
|
||||
boost::python::register_ptr_to_python<boost::shared_ptr<PTR > >(); \
|
||||
} \
|
||||
} while (0)
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1)
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2)
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3)
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3)
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3)
|
||||
|
||||
// Function pointers to desambiguate Cal3_S2::calibrate calls
|
||||
Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate;
|
||||
Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate;
|
||||
|
||||
void exportCal3_S2(){
|
||||
|
||||
class_<Cal3_S2, boost::shared_ptr<Cal3_S2> >("Cal3_S2", init<>())
|
||||
.def(init<double,double,double,double,double>())
|
||||
.def(init<const Vector &>())
|
||||
.def(init<double,int,int>(args("fov","w","h")))
|
||||
.def(init<std::string>())
|
||||
.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<Cal3_S2> >();
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include "gtsam/geometry/PinholeCamera.h"
|
||||
#include "gtsam/geometry/Cal3_S2.h"
|
||||
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;
|
||||
|
||||
// Wrapper on PinholeBaseK<Cal3_S2> because it has pure virtual method calibration()
|
||||
struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCal3_S2>
|
||||
{
|
||||
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<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
|
||||
Point2 (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::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_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
|
||||
.def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration),
|
||||
return_value_policy<copy_const_reference>())
|
||||
.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());
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include "gtsam/geometry/PinholeCamera.h"
|
||||
#include "gtsam/geometry/Cal3_S2.h"
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;
|
||||
typedef PinholeCamera<Cal3_S2> 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, bases<PinholeBaseKCal3_S2> >("PinholeCameraCal3_S2", init<>())
|
||||
.def(init<const Pose3 &>())
|
||||
.def(init<const Pose3 &, const Cal3_S2 &>())
|
||||
.def(init<const Vector &>())
|
||||
.def(init<const Vector &, const Vector &>())
|
||||
.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<copy_const_reference>())
|
||||
// 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<copy_const_reference>())
|
||||
.def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads())
|
||||
.staticmethod("Lookat")
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("Point2", init<>())
|
||||
.def(init<double, double>())
|
||||
.def(init<const Vector2 &>())
|
||||
.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<copy_const_reference>())
|
||||
.def("x", &Point2::x)
|
||||
.def("y", &Point2::y)
|
||||
.def(self * other<double>()) // __mult__
|
||||
.def(other<double>() * self) // __mult__
|
||||
.def(self + self)
|
||||
.def(-self)
|
||||
.def(self - self)
|
||||
.def(self / other<double>())
|
||||
.def(self_ns::str(self))
|
||||
.def(repr(self))
|
||||
.def(self == self)
|
||||
;
|
||||
#endif
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("Point3")
|
||||
.def(init<>())
|
||||
.def(init<double,double,double>())
|
||||
.def(init<const Vector3 &>())
|
||||
.def("vector", &Point3::vector, return_value_policy<copy_const_reference>())
|
||||
.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<double>())
|
||||
.def(other<double>() * self)
|
||||
.def(self + self)
|
||||
.def(-self)
|
||||
.def(self - self)
|
||||
.def(self / other<double>())
|
||||
.def(self_ns::str(self))
|
||||
.def(repr(self))
|
||||
.def(self == self);
|
||||
#endif
|
||||
|
||||
class_<Point3Pair>("Point3Pair", init<Point3, Point3>())
|
||||
.def_readwrite("first", &Point3Pair::first)
|
||||
.def_readwrite("second", &Point3Pair::second);
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<Matrix&>, boost::optional<Matrix&>) const
|
||||
// = &Pose2::range;
|
||||
// double (Pose2::*range2)(const Point2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
|
||||
// = &Pose2::range;
|
||||
|
||||
// Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
|
||||
// = &Pose2::bearing;
|
||||
// Rot2 (Pose2::*bearing2)(const Point2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
|
||||
// = &Pose2::bearing;
|
||||
|
||||
class_<Pose2>("Pose2", init<>())
|
||||
.def(init<Pose2>())
|
||||
.def(init<double, double, double>())
|
||||
.def(init<double, Point2>())
|
||||
.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<copy_const_reference>())
|
||||
.def("r", &Pose2::r, return_value_policy<copy_const_reference>())
|
||||
.def("translation", &Pose2::translation, return_value_policy<copy_const_reference>())
|
||||
.def("rotation", &Pose2::rotation, return_value_policy<copy_const_reference>())
|
||||
|
||||
// .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__
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("Pose3")
|
||||
.def(init<>())
|
||||
.def(init<const Pose3&>())
|
||||
.def(init<const Rot3&, const Point3&>())
|
||||
.def(init<const Rot3&, const Vector3&>())
|
||||
.def(init<const Pose2&>())
|
||||
.def(init<const Matrix&>())
|
||||
.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<copy_const_reference>()])
|
||||
.def("translation", &Pose3::translation,
|
||||
translation_overloads()[return_value_policy<copy_const_reference>()])
|
||||
.def(self * self) // __mult__
|
||||
.def(self * other<Point3>()) // __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());
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("Rot2", init<>())
|
||||
.def(init<double>())
|
||||
.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__
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("Rot3")
|
||||
.def(init<Point3,Point3,Point3>())
|
||||
.def(init<double,double,double,double,double,double,double,double,double>())
|
||||
.def(init<double,double,double,double>())
|
||||
.def(init<const Quaternion &>())
|
||||
.def(init<const Matrix3 &>())
|
||||
.def(init<const Matrix &>())
|
||||
.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<Point3>())
|
||||
.def(self * other<Unit3>())
|
||||
.def(self_ns::str(self)) // __str__
|
||||
.def(repr(self)) // __repr__
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
void export_geometry() {
|
||||
class_<Unit3>("Unit3")
|
||||
.def(init<>())
|
||||
.def(init<double, double, double>())
|
||||
.def(init<const Vector3&>());
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include <sstream> // 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<Symbol> 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<Symbol>(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, boost::shared_ptr<Symbol> >("Symbol")
|
||||
.def(init<>())
|
||||
.def(init<const Symbol &>())
|
||||
.def("__init__", make_constructor(makeSymbol))
|
||||
.def(init<Key>())
|
||||
.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<Key>())
|
||||
.def(self != self)
|
||||
.def(self != other<Key>())
|
||||
.def("__repr__", &selfToString)
|
||||
.def("__int__", &selfToKey)
|
||||
.def("chr", &chrFromSelf)
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<Base>
|
||||
{
|
||||
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<std::string>(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_<BaseCallback,boost::noncopyable>("Base")
|
||||
.def("print", pure_virtual(&Base::print))
|
||||
;
|
||||
register_ptr_to_python< boost::shared_ptr<Base> >();
|
||||
|
||||
// NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining)
|
||||
class_<Gaussian, boost::shared_ptr<Gaussian>, bases<Base> >("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_<Diagonal, boost::shared_ptr<Diagonal>, bases<Gaussian> >("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_<Isotropic, boost::shared_ptr<Isotropic>, bases<Diagonal> >("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_<Unit, boost::shared_ptr<Unit>, bases<Isotropic> >("Unit", no_init)
|
||||
.def("Create",&Unit::Create)
|
||||
.staticmethod("Create")
|
||||
;
|
||||
|
||||
REGISTER_SHARED_PTR_TO_PYTHON(Unit);
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("OptionalJacobian39", init<>());
|
||||
class_<OptionalJacobian96>("OptionalJacobian96", init<>());
|
||||
class_<OptionalJacobian9>("OptionalJacobian9", init<>());
|
||||
|
||||
class_<NavState>("NavState", init<>())
|
||||
.def(init<const Rot3&, const Point3&, const Velocity3&>())
|
||||
// TODO(frank): overload with jacobians
|
||||
.def("print", &NavState::print, print_overloads())
|
||||
.def("attitude", &NavState::attitude,
|
||||
attitude_overloads()[return_value_policy<copy_const_reference>()])
|
||||
.def("position", &NavState::position,
|
||||
position_overloads()[return_value_policy<copy_const_reference>()])
|
||||
.def("velocity", &NavState::velocity,
|
||||
velocity_overloads()[return_value_policy<copy_const_reference>()])
|
||||
.def(repr(self))
|
||||
.def("pose", &NavState::pose);
|
||||
|
||||
class_<imuBias::ConstantBias>("ConstantBias", init<>())
|
||||
.def(init<const Vector3&, const Vector3&>())
|
||||
.def(repr(self));
|
||||
|
||||
class_<PreintegrationParams, boost::shared_ptr<PreintegrationParams>>(
|
||||
"PreintegrationParams", init<const Vector3&>())
|
||||
.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_<PreintegrationType>(
|
||||
#ifdef GTSAM_TANGENT_PREINTEGRATION
|
||||
"TangentPreintegration",
|
||||
#else
|
||||
"ManifoldPreintegration",
|
||||
#endif
|
||||
init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>())
|
||||
.def("predict", &PreintegrationType::predict, predict_overloads())
|
||||
.def("computeError", &PreintegrationType::computeError)
|
||||
.def("resetIntegration", &PreintegrationType::resetIntegration)
|
||||
.def("deltaTij", &PreintegrationType::deltaTij);
|
||||
|
||||
class_<PreintegratedImuMeasurements, bases<PreintegrationType>>(
|
||||
"PreintegratedImuMeasurements",
|
||||
init<const boost::shared_ptr<PreintegrationParams>&, 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_<ImuFactor, bases<NonlinearFactor>, boost::shared_ptr<ImuFactor>>("ImuFactor")
|
||||
.def("error", &ImuFactor::error)
|
||||
.def(init<Key, Key, Key, Key, Key, const PreintegratedImuMeasurements&>())
|
||||
.def(repr(self));
|
||||
REGISTER_SHARED_PTR_TO_PYTHON(ImuFactor);
|
||||
|
||||
class_<ImuFactor2, bases<NonlinearFactor>, boost::shared_ptr<ImuFactor2>>("ImuFactor2")
|
||||
.def("error", &ImuFactor2::error)
|
||||
.def(init<Key, Key, Key, const PreintegratedImuMeasurements&>())
|
||||
.def(repr(self));
|
||||
REGISTER_SHARED_PTR_TO_PYTHON(ImuFactor2);
|
||||
|
||||
class_<GPSFactor, bases<NonlinearFactor>, boost::shared_ptr<GPSFactor>>("GPSFactor")
|
||||
.def("error", &GPSFactor::error)
|
||||
.def(init<Key, const Point3&, noiseModel::Base::shared_ptr>());
|
||||
REGISTER_SHARED_PTR_TO_PYTHON(GPSFactor);
|
||||
|
||||
class_<GPSFactor2, bases<NonlinearFactor>, boost::shared_ptr<GPSFactor2>>("GPSFactor2")
|
||||
.def("error", &GPSFactor2::error)
|
||||
.def(init<Key, const Point3&, noiseModel::Base::shared_ptr>());
|
||||
REGISTER_SHARED_PTR_TO_PYTHON(GPSFactor2);
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<const Scenario*>(&scenario);
|
||||
}
|
||||
|
||||
void exportScenario() {
|
||||
// NOTE(frank): Abstract classes need boost::noncopyable
|
||||
class_<Scenario, boost::noncopyable>("Scenario", no_init);
|
||||
|
||||
// TODO(frank): figure out how to do inheritance
|
||||
class_<ConstantTwistScenario>("ConstantTwistScenario",
|
||||
init<const Vector3&, const Vector3&>())
|
||||
.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<reference_existing_object>());
|
||||
|
||||
class_<ScenarioRunner>(
|
||||
"ScenarioRunner",
|
||||
init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,
|
||||
double, const imuBias::ConstantBias&>())
|
||||
.def("actualSpecificForce", &ScenarioRunner::actualSpecificForce)
|
||||
.def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity)
|
||||
.def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce)
|
||||
.def("imuSampleTime", &ScenarioRunner::imuSampleTime,
|
||||
return_value_policy<copy_const_reference>())
|
||||
.def("integrate", &ScenarioRunner::integrate)
|
||||
.def("predict", &ScenarioRunner::predict)
|
||||
.def("estimateCovariance", &ScenarioRunner::estimateCovariance);
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("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>("ISAM2Result")
|
||||
;
|
||||
|
||||
// Function pointers for overloads in ISAM2
|
||||
Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate;
|
||||
|
||||
class_<ISAM2>("ISAM2")
|
||||
.def(init<const ISAM2Params &>())
|
||||
// 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<Pose3>, (arg("self"), arg("key")) )
|
||||
.def("value_exists", &ISAM2::valueExists)
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -1,42 +0,0 @@
|
|||
#include <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
|
||||
void exportLevenbergMarquardtOptimizer() {
|
||||
class_<GaussNewtonParams>("GaussNewtonParams", init<>())
|
||||
.def_readwrite("maxIterations", &GaussNewtonParams::maxIterations)
|
||||
.def_readwrite("relativeErrorTol", &GaussNewtonParams::relativeErrorTol);
|
||||
|
||||
class_<GaussNewtonOptimizer, boost::noncopyable>(
|
||||
"GaussNewtonOptimizer",
|
||||
init<const NonlinearFactorGraph&, const Values&, const GaussNewtonParams&>())
|
||||
.def("optimize", &GaussNewtonOptimizer::optimize,
|
||||
return_value_policy<copy_const_reference>());
|
||||
|
||||
class_<LevenbergMarquardtParams>("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, boost::noncopyable>(
|
||||
"LevenbergMarquardtOptimizer",
|
||||
init<const NonlinearFactorGraph&, const Values&, const LevenbergMarquardtParams&>())
|
||||
.def("optimize", &LevenbergMarquardtOptimizer::optimize,
|
||||
return_value_policy<copy_const_reference>());
|
||||
|
||||
class_<Marginals>("Marginals", init<const NonlinearFactorGraph&, const Values&>())
|
||||
.def("marginalCovariance", &Marginals::marginalCovariance);
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<NonlinearFactor>
|
||||
{
|
||||
double error (const Values & values) const {
|
||||
return this->get_override("error")(values);
|
||||
}
|
||||
size_t dim () const {
|
||||
return this->get_override("dim")();
|
||||
}
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Values & values) const {
|
||||
return this->get_override("linearize")(values);
|
||||
}
|
||||
};
|
||||
|
||||
// Similarly for NoiseModelFactor:
|
||||
struct NoiseModelFactorWrap : NoiseModelFactor, wrapper<NoiseModelFactor> {
|
||||
// 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<GaussianFactor> linearize(const Values & values) const {
|
||||
return this->get_override("linearize")(values);
|
||||
}
|
||||
Vector unwhitenedError(const Values& x,
|
||||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||
return this->get_override("unwhitenedError")(x, H);
|
||||
}
|
||||
};
|
||||
|
||||
void exportNonlinearFactor() {
|
||||
class_<NonlinearFactorWrap, boost::noncopyable>("NonlinearFactor")
|
||||
.def("error", pure_virtual(&NonlinearFactor::error))
|
||||
.def("dim", pure_virtual(&NonlinearFactor::dim))
|
||||
.def("linearize", pure_virtual(&NonlinearFactor::linearize));
|
||||
register_ptr_to_python<boost::shared_ptr<NonlinearFactor> >();
|
||||
|
||||
class_<NoiseModelFactorWrap, boost::noncopyable>("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<boost::shared_ptr<NoiseModelFactor> >();
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<NonlinearFactor> getNonlinearFactor(
|
||||
const NonlinearFactorGraph& graph, size_t idx) {
|
||||
auto p = boost::dynamic_pointer_cast<NonlinearFactor>(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>("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);
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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>("Values", init<>())
|
||||
.def(init<Values>())
|
||||
.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<Point2>)
|
||||
.def("atRot2", &Values::at<Rot2>)
|
||||
.def("atPose2", &Values::at<Pose2>)
|
||||
.def("atPoint3", &Values::at<Point3>)
|
||||
.def("atRot3", &Values::at<Rot3>)
|
||||
.def("atPose3", &Values::at<Pose3>)
|
||||
.def("atConstantBias", &Values::at<Bias>)
|
||||
.def("atNavState", &Values::at<NavState>)
|
||||
.def("atVector", &Values::at<Vector>)
|
||||
.def("atVector2", &Values::at<Vector2>)
|
||||
.def("atVector3", &Values::at<Vector3>)
|
||||
.def("exists", exists1)
|
||||
.def("keys", &Values::keys)
|
||||
;
|
||||
}
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
#include <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include <gtsam/sam/BearingFactor.h>
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
|
||||
using namespace std;
|
||||
|
||||
template <class VALUE>
|
||||
void exportBearingFactor(const std::string& name) {
|
||||
class_<VALUE>(name, init<>());
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<class T>
|
||||
// void exportBetweenFactor(const std::string& name){
|
||||
// class_<T>(name, init<>())
|
||||
// .def(init<Key, Key, T, SharedNoiseModel>())
|
||||
// ;
|
||||
// }
|
||||
|
||||
#define BETWEENFACTOR(T) \
|
||||
class_< BetweenFactor<T>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<T> > >("BetweenFactor"#T) \
|
||||
.def(init<Key,Key,T,noiseModel::Base::shared_ptr>()) \
|
||||
.def("measured", &BetweenFactor<T>::measured, return_internal_reference<>()) \
|
||||
;
|
||||
|
||||
void exportBetweenFactors()
|
||||
{
|
||||
BETWEENFACTOR(Point2)
|
||||
BETWEENFACTOR(Rot2)
|
||||
BETWEENFACTOR(Pose2)
|
||||
BETWEENFACTOR(Point3)
|
||||
BETWEENFACTOR(Rot3)
|
||||
BETWEENFACTOR(Pose3)
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<Pose3, Point3, Cal3_S2> 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, bases<NonlinearFactor> >("GenericProjectionFactorCal3_S2", init<>())
|
||||
.def(init<const Point2 &, SharedNoiseModel, Key, Key, const boost::shared_ptr<Cal3_S2> &, optional<Pose3> >())
|
||||
.def(init<const Point2 &, SharedNoiseModel, Key, Key, const boost::shared_ptr<Cal3_S2> &, bool, bool, optional<Pose3> >())
|
||||
.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<copy_const_reference>())
|
||||
// TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &'
|
||||
// .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy<copy_const_reference>())
|
||||
.def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality)
|
||||
.def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality)
|
||||
;
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<VALUE>, bases<NonlinearFactor>, boost::shared_ptr< PriorFactor<VALUE> > >("PriorFactor"#VALUE) \
|
||||
.def(init<Key,VALUE,noiseModel::Base::shared_ptr>()) \
|
||||
.def("prior", &PriorFactor<VALUE>::prior, return_internal_reference<>()) \
|
||||
;
|
||||
|
||||
void exportPriorFactors()
|
||||
{
|
||||
PRIORFACTOR(Point2)
|
||||
PRIORFACTOR(Rot2)
|
||||
PRIORFACTOR(Pose2)
|
||||
PRIORFACTOR(Point3)
|
||||
PRIORFACTOR(Rot3)
|
||||
PRIORFACTOR(Pose3)
|
||||
PRIORFACTOR(Vector3)
|
||||
PRIORFACTOR(NavState)
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#include <gtsam/slam/RotateFactor.h>
|
||||
|
||||
using namespace boost::python;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
void export_slam() {
|
||||
class_<RotateDirectionsFactor, bases<NonlinearFactor>>(
|
||||
"RotateDirectionsFactor",
|
||||
init<Key, const Unit3&, const Unit3&, noiseModel::Base::shared_ptr>())
|
||||
.def("Initialize", &RotateDirectionsFactor::Initialize)
|
||||
.staticmethod("Initialize");
|
||||
}
|
||||
|
|
@ -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 <boost/python.hpp>
|
||||
|
||||
#define NO_IMPORT_ARRAY
|
||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||
|
||||
#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<Vector>::register_converter();
|
||||
NumpyEigenConverter<Vector1>::register_converter();
|
||||
NumpyEigenConverter<Vector2>::register_converter();
|
||||
NumpyEigenConverter<Vector3>::register_converter();
|
||||
NumpyEigenConverter<Vector4>::register_converter();
|
||||
NumpyEigenConverter<Vector5>::register_converter();
|
||||
NumpyEigenConverter<Vector6>::register_converter();
|
||||
NumpyEigenConverter<Vector7>::register_converter();
|
||||
NumpyEigenConverter<Vector8>::register_converter();
|
||||
NumpyEigenConverter<Vector9>::register_converter();
|
||||
NumpyEigenConverter<Vector10>::register_converter();
|
||||
|
||||
NumpyEigenConverter<Matrix>::register_converter();
|
||||
NumpyEigenConverter<Matrix2>::register_converter();
|
||||
NumpyEigenConverter<Matrix3>::register_converter();
|
||||
NumpyEigenConverter<Matrix4>::register_converter();
|
||||
NumpyEigenConverter<Matrix5>::register_converter();
|
||||
NumpyEigenConverter<Matrix6>::register_converter();
|
||||
NumpyEigenConverter<Matrix7>::register_converter();
|
||||
NumpyEigenConverter<Matrix8>::register_converter();
|
||||
NumpyEigenConverter<Matrix9>::register_converter();
|
||||
|
||||
}
|
||||
|
|
@ -1,334 +0,0 @@
|
|||
/**
|
||||
* @file NumpyEigenConverter.hpp
|
||||
* @author Paul Furgale <paul.furgale@utoronto.ca>
|
||||
* @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 <numpy_eigen/boost_python_headers.hpp>
|
||||
//#include <iostream>
|
||||
|
||||
#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 <numpy/arrayobject.h>
|
||||
|
||||
#include "type_traits.hpp"
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#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 <NumpyEigenConverter.hpp>
|
||||
*
|
||||
*
|
||||
* BOOST_PYTHON_MODULE(libmy_module_python)
|
||||
* {
|
||||
* // The converters will cause a segfault unless import_array() is called before the first one
|
||||
* import_array();
|
||||
* NumpyEigenConverter<Eigen::Matrix< double, 1, 1 > >::register_converter();
|
||||
* NumpyEigenConverter<Eigen::Matrix< double, 2, 1 > >::register_converter();
|
||||
* }
|
||||
*
|
||||
*/
|
||||
template<typename EIGEN_MATRIX_T>
|
||||
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<scalar_t>::NpyType,
|
||||
//Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret,
|
||||
//CoeffReadCost = NumTraits<Scalar>::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<std::string>(option);
|
||||
}
|
||||
|
||||
static std::string toString()
|
||||
{
|
||||
return std::string() + "Eigen::Matrix<" + TypeToNumPy<scalar_t>::typeString() + ", " +
|
||||
castSizeOption(RowsAtCompileTime) + ", " +
|
||||
castSizeOption(ColsAtCompileTime) + ", " +
|
||||
boost::lexical_cast<std::string>((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<scalar_t>::NpyType);
|
||||
numpyTypeDemuxer< CopyEigenToNumpyVector<const matrix_t> >(&M, reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(P));
|
||||
}
|
||||
else
|
||||
{
|
||||
// create a 2D array.
|
||||
npy_intp dimensions[2];
|
||||
dimensions[0] = M.rows();
|
||||
dimensions[1] = M.cols();
|
||||
P = PyArray_SimpleNew(2, dimensions, TypeToNumPy<scalar_t>::NpyType);
|
||||
numpyTypeDemuxer< CopyEigenToNumpyMatrix<const matrix_t> >(&M, reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(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<NPE_PY_ARRAY_OBJECT*>(obj_ptr);
|
||||
|
||||
// Check the type of the array.
|
||||
int npyType = getNpyType(array_ptr);
|
||||
|
||||
if(!TypeToNumPy<scalar_t>::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<matrix_t> * matData = reinterpret_cast<boost::python::converter::rvalue_from_python_storage<matrix_t> * >(data);
|
||||
void* storage = matData->storage.bytes;
|
||||
|
||||
// Make sure storage is 16byte aligned. With help from code from Memory.h
|
||||
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(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<NPE_PY_ARRAY_OBJECT*>(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<matrix_t> >(&M, array_ptr);
|
||||
}
|
||||
else
|
||||
{
|
||||
int rows = PyArray_DIM(array_ptr, 0);
|
||||
int cols = PyArray_DIM(array_ptr, 1);
|
||||
|
||||
M.resize(rows,cols);
|
||||
numpyTypeDemuxer< CopyNumpyToEigenMatrix<matrix_t> >(&M, array_ptr);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
// The registration function.
|
||||
static void register_converter()
|
||||
{
|
||||
boost::python::to_python_converter<matrix_t,NumpyEigenConverter>();
|
||||
boost::python::converter::registry::push_back(
|
||||
&NumpyEigenConverter::convertible,
|
||||
&NumpyEigenConverter::construct,
|
||||
boost::python::type_id<matrix_t>());
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* NUMPY_EIGEN_CONVERTER_HPP */
|
||||
|
|
@ -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)
|
||||
|
|
@ -1,250 +0,0 @@
|
|||
/**
|
||||
* @file boost_python_headers.hpp
|
||||
* @author Paul Furgale <paul.furgale@gmail.com>
|
||||
* @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 <Eigen/Core>
|
||||
#include <boost/python.hpp>
|
||||
#include <boost/python/detail/referent_storage.hpp>
|
||||
#include <boost/python/converter/arg_from_python.hpp>
|
||||
#include <boost/python/converter/rvalue_from_python_data.hpp>
|
||||
#include <boost/python/tuple.hpp>
|
||||
|
||||
|
||||
namespace boost { namespace python { namespace detail {
|
||||
template<typename T>
|
||||
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<typename T, int A, int B, int C, int D, int E>
|
||||
struct referent_size< Eigen::Matrix<T,A,B,C,D,E>& >
|
||||
{
|
||||
// Add 16 bytes so we can get alignment
|
||||
BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
|
||||
};
|
||||
|
||||
// This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
|
||||
template<typename T, int A, int B, int C, int D, int E>
|
||||
struct referent_size< Eigen::Matrix<T,A,B,C,D,E> const & >
|
||||
{
|
||||
// Add 16 bytes so we can get alignment
|
||||
BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
|
||||
};
|
||||
|
||||
// This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
|
||||
template<typename T, int A, int B, int C, int D, int E>
|
||||
struct referent_size< Eigen::Matrix<T,A,B,C,D,E> >
|
||||
{
|
||||
// Add 16 bytes so we can get alignment
|
||||
BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
|
||||
};
|
||||
|
||||
|
||||
}}}
|
||||
|
||||
namespace boost { namespace python { namespace converter {
|
||||
|
||||
|
||||
template<typename S, int A, int B, int C, int D, int E>
|
||||
struct rvalue_from_python_data< Eigen::Matrix<S,A,B,C,D,E> const &> : rvalue_from_python_storage< Eigen::Matrix<S,A,B,C,D,E> const & >
|
||||
{
|
||||
typedef typename Eigen::Matrix<S,A,B,C,D,E> 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<T>,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<void *>(this->storage.bytes);
|
||||
T * aligned = reinterpret_cast<T *>(reinterpret_cast<void *>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16));
|
||||
|
||||
//std::cout << "Destroying " << (void*)aligned << std::endl;
|
||||
aligned->T::~T();
|
||||
}
|
||||
}
|
||||
private:
|
||||
typedef typename add_reference<typename add_cv<T>::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<typename S, int A, int B, int C, int D, int E>
|
||||
struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> >
|
||||
{
|
||||
typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
|
||||
typedef typename boost::add_reference<
|
||||
T
|
||||
// We can't add_const here, or it would be impossible to pass
|
||||
// auto_ptr<U> args from Python to C++
|
||||
>::type result_type;
|
||||
|
||||
arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::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<T>::
|
||||
# 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<void *>(m_data.storage.bytes);
|
||||
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
|
||||
|
||||
return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
|
||||
}
|
||||
|
||||
private:
|
||||
rvalue_from_python_data<result_type> 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<typename S, int A, int B, int C, int D, int E>
|
||||
struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> const & >
|
||||
{
|
||||
typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
|
||||
typedef typename boost::add_reference<
|
||||
T
|
||||
// We can't add_const here, or it would be impossible to pass
|
||||
// auto_ptr<U> args from Python to C++
|
||||
>::type result_type;
|
||||
|
||||
arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::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<T>::
|
||||
# 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<void *>(m_data.storage.bytes);
|
||||
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
|
||||
|
||||
return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
|
||||
}
|
||||
|
||||
private:
|
||||
rvalue_from_python_data<result_type> 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<typename S, int A, int B, int C, int D, int E>
|
||||
struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> const >
|
||||
{
|
||||
typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
|
||||
typedef typename boost::add_reference<
|
||||
T
|
||||
// We can't add_const here, or it would be impossible to pass
|
||||
// auto_ptr<U> args from Python to C++
|
||||
>::type result_type;
|
||||
|
||||
arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::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<T>::
|
||||
# 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<void *>(m_data.storage.bytes);
|
||||
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
|
||||
|
||||
return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
|
||||
}
|
||||
|
||||
private:
|
||||
rvalue_from_python_data<result_type> m_data;
|
||||
PyObject* m_source;
|
||||
};
|
||||
|
||||
|
||||
}}}
|
||||
|
||||
|
||||
#endif /* NUMPY_EIGEN_CONVERTERS_HPP */
|
||||
|
|
@ -1,149 +0,0 @@
|
|||
#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP
|
||||
#define NUMPY_EIGEN_COPY_ROUTINES_HPP
|
||||
|
||||
|
||||
template<typename EIGEN_T>
|
||||
struct CopyNumpyToEigenMatrix
|
||||
{
|
||||
typedef EIGEN_T matrix_t;
|
||||
typedef typename matrix_t::Scalar scalar_t;
|
||||
|
||||
template<typename T>
|
||||
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<T*>(PyArray_GETPTR2(P_, r, c));
|
||||
(*M_)(r,c) = static_cast<scalar_t>(*p);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template<typename EIGEN_T>
|
||||
struct CopyEigenToNumpyMatrix
|
||||
{
|
||||
typedef EIGEN_T matrix_t;
|
||||
typedef typename matrix_t::Scalar scalar_t;
|
||||
|
||||
template<typename T>
|
||||
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<T*>(PyArray_GETPTR2(P_, r, c));
|
||||
*p = static_cast<T>((*M_)(r,c));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template<typename EIGEN_T>
|
||||
struct CopyEigenToNumpyVector
|
||||
{
|
||||
typedef EIGEN_T matrix_t;
|
||||
typedef typename matrix_t::Scalar scalar_t;
|
||||
|
||||
template<typename T>
|
||||
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<T*>(PyArray_GETPTR1(P_, i));
|
||||
*p = static_cast<T>((*M_)(i));
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
template<typename EIGEN_T>
|
||||
struct CopyNumpyToEigenVector
|
||||
{
|
||||
typedef EIGEN_T matrix_t;
|
||||
typedef typename matrix_t::Scalar scalar_t;
|
||||
|
||||
template<typename T>
|
||||
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<T*>(PyArray_GETPTR1(P_, i));
|
||||
(*M_)(i) = static_cast<scalar_t>(*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<bool>(M,P);
|
||||
break;
|
||||
case NPY_BYTE:
|
||||
f.template exec<char>(M,P);
|
||||
break;
|
||||
case NPY_UBYTE:
|
||||
f.template exec<unsigned char>(M,P);
|
||||
break;
|
||||
case NPY_SHORT:
|
||||
f.template exec<short>(M,P);
|
||||
break;
|
||||
case NPY_USHORT:
|
||||
f.template exec<unsigned short>(M,P);
|
||||
break;
|
||||
case NPY_INT:
|
||||
f.template exec<int>(M,P);
|
||||
break;
|
||||
case NPY_UINT:
|
||||
f.template exec<unsigned int>(M,P);
|
||||
break;
|
||||
case NPY_LONG:
|
||||
f.template exec<long>(M,P);
|
||||
break;
|
||||
case NPY_ULONG:
|
||||
f.template exec<unsigned long>(M,P);
|
||||
break;
|
||||
case NPY_LONGLONG:
|
||||
f.template exec<long long>(M,P);
|
||||
break;
|
||||
case NPY_ULONGLONG:
|
||||
f.template exec<unsigned long long>(M,P);
|
||||
break;
|
||||
case NPY_FLOAT:
|
||||
f.template exec<float>(M,P);
|
||||
break;
|
||||
case NPY_DOUBLE:
|
||||
f.template exec<double>(M,P);
|
||||
break;
|
||||
case NPY_LONGDOUBLE:
|
||||
f.template exec<long double>(M,P);
|
||||
break;
|
||||
default:
|
||||
THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */
|
||||
|
|
@ -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<typename Scalar> struct TypeToNumPy;
|
||||
|
||||
template<> struct TypeToNumPy<int>
|
||||
{
|
||||
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<boost::int64_t>
|
||||
{
|
||||
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<boost::uint64_t>
|
||||
{
|
||||
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<unsigned char>
|
||||
{
|
||||
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<char>
|
||||
{
|
||||
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<float>
|
||||
{
|
||||
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<double>
|
||||
{
|
||||
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 */
|
||||
|
|
@ -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
|
||||
)
|
||||
Loading…
Reference in New Issue