diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 067929a20..35ec4f66d 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -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 diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py new file mode 100644 index 000000000..3a64e0cdb --- /dev/null +++ b/cython/gtsam/examples/SFMExample.py @@ -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() diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py new file mode 100644 index 000000000..0c3ac467f --- /dev/null +++ b/cython/gtsam/examples/SimpleRotation.py @@ -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() diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py new file mode 100644 index 000000000..23b880bec --- /dev/null +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -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()