Matthew Broadway
Frank Dellaert
commit
5a0ef2caac
1
THANKS
1
THANKS
|
|
@ -27,6 +27,7 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, li
|
|||
* Natesh Srinivasan
|
||||
* Alex Trevor
|
||||
* Stephen Williams, BossaNova
|
||||
* Matthew Broadway
|
||||
|
||||
at ETH, Zurich
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,57 @@
|
|||
These examples are almost identical to the old handwritten python wrapper
|
||||
examples. However, there are just some slight name changes, for example
|
||||
noiseModel.Diagonal becomes noiseModel_Diagonal etc...
|
||||
Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0))
|
||||
`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
|
||||
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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
"""
|
||||
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
|
||||
"""
|
||||
from __future__ import print_function
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.examples import SFMdata
|
||||
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
|
||||
GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
|
||||
Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
|
||||
Rot3, SimpleCamera, Values)
|
||||
|
||||
|
||||
def symbol(name: str, index: int) -> int:
|
||||
""" helper for creating a symbol without explicitly casting 'name' from str to int """
|
||||
return gtsam.symbol(ord(name), index)
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
|
||||
# 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.
|
||||
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
|
||||
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()
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
"""
|
||||
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 numpy as np
|
||||
import gtsam
|
||||
|
||||
|
||||
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()
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
"""
|
||||
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
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
import gtsam
|
||||
from gtsam.examples import SFMdata
|
||||
from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
|
||||
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
|
||||
PriorFactorPoint3, PriorFactorPose3, Rot3,
|
||||
SimpleCamera, Values)
|
||||
|
||||
|
||||
def symbol(name: str, index: int) -> int:
|
||||
""" helper for creating a symbol without explicitly casting 'name' from str to int """
|
||||
return gtsam.symbol(ord(name), index)
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
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
|
||||
"""
|
||||
|
||||
# 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 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
|
||||
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()
|
||||
Loading…
Reference in New Issue