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

started porting more examples to python

release/4.3a0
mbway 2019-03-10 16:25:42 +00:00
parent 20c4af4ec6
commit 8a81364daf
No known key found for this signature in database
GPG Key ID: DDC0B82B6896B381
4 changed files with 353 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
cython/gtsam/examples/README.md
View File

@ -1,4 +1,57 @@
These examples are almost identical to the old handwritten python wrapper These examples are almost identical to the old handwritten python wrapper
examples. However, there are just some slight name changes, for example examples. However, there are just some slight name changes, for example
noiseModel.Diagonal becomes noiseModel_Diagonal etc... `noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0)) Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))`
# Porting Progress
| C++ Example Name | Ported |
|-------------------------------------------------------|--------|
| CameraResectioning | |
| CreateSFMExampleData | |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython |
| ImuFactorExample2 | X |
| ImuFactorsExample | |
| ISAM2Example_SmartFactor | |
| ISAM2_SmartFactorStereo_IMU | |
| LocalizationExample | impossible? |
| METISOrderingExample | |
| OdometryExample | X |
| PlanarSLAMExample | X |
| Pose2SLAMExample | X |
| Pose2SLAMExampleExpressions | |
| Pose2SLAMExample_g2o | X |
| Pose2SLAMExample_graph | |
| Pose2SLAMExample_graphviz | |
| Pose2SLAMExample_lago | lago not exposed through cython |
| Pose2SLAMStressTest | |
| Pose2SLAMwSPCG | |
| Pose3SLAMExample_changeKeys | |
| Pose3SLAMExampleExpressions_BearingRangeWithTransform | |
| Pose3SLAMExample_g2o | X |
| Pose3SLAMExample_initializePose3Chordal | |
| Pose3SLAMExample_initializePose3Gradient | |
| RangeISAMExample_plaza2 | |
| SelfCalibrationExample | |
| SFMExample_bal_COLAMD_METIS | |
| SFMExample_bal | |
| SFMExample | X |
| SFMExampleExpressions_bal | |
| SFMExampleExpressions | |
| SFMExample_SmartFactor | |
| SFMExample_SmartFactorPCG | |
| SimpleRotation | X |
| SolverComparer | |
| StereoVOExample | |
| StereoVOExample_large | |
| TimeTBB | |
| UGM_chain | discrete functionality not exposed |
| UGM_small | discrete functionality not exposed |
| VisualISAM2Example | X |
| VisualISAMExample | X |
Extra Examples (with no C++ equivalent)
- PlanarManipulatorExample
- SFMData

113
cython/gtsam/examples/SFMExample.py Normal file
View File

@ -0,0 +1,113 @@
"""
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
A structure-from-motion problem on a simulated dataset
"""
import gtsam
import numpy as np
from gtsam.examples import SFMdata
from gtsam.gtsam import Values, NonlinearFactorGraph, PriorFactorPose3, SimpleCamera, \
GenericProjectionFactorCal3_S2, \
PriorFactorPoint3, Pose3, Rot3, Point3, DoglegOptimizer, Cal3_S2
# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
#
# Each variable in the system (poses and landmarks) must be identified with a unique key.
# We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
# Here we will use Symbols
#
# In GTSAM, measurement functions are represented as 'factors'. Several common factors
# have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
# Here we will use Projection factors to model the camera's landmark observations.
# Also, we will initialize the robot at some location using a Prior factor.
#
# When the factors are created, we will add them to a Factor Graph. As the factors we are using
# are nonlinear factors, we will need a Nonlinear Factor Graph.
#
# Finally, once all of the factors have been added to our factor graph, we will want to
# solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
# GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
# trust-region method known as Powell's Degleg
#
# The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
# nonlinear functions around an initial linearization point, then solve the linear system
# to update the linearization point. This happens repeatedly until the solver converges
# to a consistent set of variable values. This requires us to specify an initial guess
# for each variable, held in a Values container.
def symbol(name, index):
return gtsam.symbol(ord(name), index)
def main():
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = 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(K)
# Create a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
t = Point3(0.05, -0.10, 0.20)
transformed_pose = pose.compose(Pose3(r, t))
initial_estimate.insert(symbol('x', i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
initial_estimate.insert(symbol('l', j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
if __name__ == '__main__':
main()

85
cython/gtsam/examples/SimpleRotation.py Normal file
View File

@ -0,0 +1,85 @@
"""
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
This example will perform a relatively trivial optimization on
a single variable with a single factor.
"""
import gtsam
import numpy as np
def main():
"""
Step 1: Create a factor to express a unary constraint
The "prior" in this case is the measurement from a sensor,
with a model of the noise on the measurement.
The "Key" created here is a label used to associate parts of the
state (stored in "RotValues") with particular factors. They require
an index to allow for lookup, and should be unique.
In general, creating a factor requires:
- A key or set of keys labeling the variables that are acted upon
- A measurement value
- A measurement model with the correct dimensionality for the factor
"""
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle')
model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = gtsam.symbol(ord('x'), 1)
factor = gtsam.PriorFactorRot2(key, prior, model)
"""
Step 2: Create a graph container and add the factor to it
Before optimizing, all factors need to be added to a Graph container,
which provides the necessary top-level functionality for defining a
system of constraints.
In this case, there is only one factor, but in a practical scenario,
many more factors would be added.
"""
graph = gtsam.NonlinearFactorGraph()
graph.push_back(factor)
graph.print_('full graph')
"""
Step 3: Create an initial estimate
An initial estimate of the solution for the system is necessary to
start optimization. This system state is the "Values" instance,
which is similar in structure to a dictionary, in that it maps
keys (the label created in step 1) to specific values.
The initial estimate provided to optimization will be used as
a linearization point for optimization, so it is important that
all of the variables in the graph have a corresponding value in
this structure.
"""
initial = gtsam.Values()
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
initial.print_('initial estimate')
"""
Step 4: Optimize
After formulating the problem with a graph of constraints
and an initial estimate, executing optimization is as simple
as calling a general optimization function with the graph and
initial estimate. This will yield a new RotValues structure
with the final state of the optimization.
"""
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
result.print_('final result')
if __name__ == '__main__':
main()

100
cython/gtsam/examples/VisualISAMExample.py Normal file
View File

@ -0,0 +1,100 @@
"""
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
A visualSLAM example for the structure-from-motion problem on a simulated dataset
This version uses iSAM to solve the problem incrementally
"""
# 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
from gtsam.gtsam import Values, Cal3_S2, NonlinearISAM, NonlinearFactorGraph, SimpleCamera, Pose3, Rot3, Point3, \
PriorFactorPose3, PriorFactorPoint3, GenericProjectionFactorCal3_S2
import numpy as np
from gtsam.examples import SFMdata
def symbol(name, index):
return gtsam.symbol(ord(name), index)
def main():
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = 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(K)
# Create a NonlinearISAM object which will relinearize and reorder the variables
# every "reorderInterval" updates
isam = NonlinearISAM(reorderInterval=3)
# Create a Factor Graph and Values to hold the new data
graph = NonlinearFactorGraph()
initial_estimate = Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(symbol('x', i), initial_xi)
# 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, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j].vector() + noise
initial_estimate.insert(symbol('l', j), Point3(initial_lj))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
if __name__ == '__main__':
main()