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

removed python

release/4.3a0
dellaert 2019-05-19 11:30:32 -04:00
parent 20f29fba0c
commit e88282c31a
52 changed files with 0 additions and 3694 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
python/.gitignore vendored
View File

@ -1 +0,0 @@
build/

96
python/CMakeLists.txt
View File

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

26
python/README.md
View File

@ -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
python/gtsam/__init__.py
View File

@ -1 +0,0 @@
from _gtsampy import *

123
python/gtsam_examples/ImuFactorExample.py
View File

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

36
python/gtsam_examples/OdometryExample.py
View File

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

68
python/gtsam_examples/Pose2SLAMExample.py
View File

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

129
python/gtsam_examples/PreintegrationExample.py
View File

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

32
python/gtsam_examples/SFMdata.py
View File

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

132
python/gtsam_examples/VisualISAM2Example.py
View File

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

2
python/gtsam_examples/__init__.py
View File

@ -1,2 +0,0 @@
from . import SFMdata
from . import VisualISAM2Example

1
python/gtsam_tests/__init__.py
View File

@ -1 +0,0 @@

13
python/gtsam_tests/testPoint2.py
View File

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

32
python/gtsam_tests/testScenario.py
View File

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

30
python/gtsam_tests/testScenarioRunner.py
View File

@ -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
python/gtsam_utils/__init__.py
View File

@ -1 +0,0 @@
from .plot import *

62
python/gtsam_utils/plot.py
View File

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

41
python/handwritten/CMakeLists.txt
View File

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

37
python/handwritten/base/FastVector.cpp
View File

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

31
python/handwritten/common.h
View File

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

107
python/handwritten/exportgtsam.cpp
View File

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

64
python/handwritten/geometry/Cal3_S2.cpp
View File

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

73
python/handwritten/geometry/PinholeBaseK.cpp
View File

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

51
python/handwritten/geometry/PinholeCamera.cpp
View File

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

61
python/handwritten/geometry/Point2.cpp
View File

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

68
python/handwritten/geometry/Point3.cpp
View File

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

93
python/handwritten/geometry/Pose2.cpp
View File

@ -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__
;
}

95
python/handwritten/geometry/Pose3.cpp
View File

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

65
python/handwritten/geometry/Rot2.cpp
View File

@ -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__
;
}

115
python/handwritten/geometry/Rot3.cpp
View File

@ -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__
;
}

35
python/handwritten/geometry/export_geometry.cpp
View File

@ -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&>());
}

83
python/handwritten/inference/Symbol.cpp
View File

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

144
python/handwritten/linear/NoiseModel.cpp
View File

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

127
python/handwritten/navigation/ImuFactor.cpp
View File

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

64
python/handwritten/navigation/Scenario.cpp
View File

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

67
python/handwritten/nonlinear/ISAM2.cpp
View File

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

42
python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

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

75
python/handwritten/nonlinear/NonlinearFactor.cpp
View File

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

60
python/handwritten/nonlinear/NonlinearFactorGraph.cpp
View File

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

89
python/handwritten/nonlinear/Values.cpp
View File

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

16
python/handwritten/slam/BearingFactor.cpp
View File

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

57
python/handwritten/slam/BetweenFactor.cpp
View File

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

54
python/handwritten/slam/GenericProjectionFactor.cpp
View File

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

60
python/handwritten/slam/PriorFactor.cpp
View File

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

36
python/handwritten/slam/export_slam.cpp
View File

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

54
python/handwritten/utils/NumpyEigen.cpp
View File

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

334
python/include/numpy_eigen/NumpyEigenConverter.hpp
View File

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

6
python/include/numpy_eigen/README.md
View File

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

250
python/include/numpy_eigen/boost_python_headers.hpp
View File

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

149
python/include/numpy_eigen/copy_routines.hpp
View File

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

191
python/include/numpy_eigen/type_traits.hpp
View File

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

15
python/setup.py.in
View File

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