From 7b4266ed6be3b601b04dc7edba1df9957a270321 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 27 Jul 2020 14:35:28 -0400 Subject: [PATCH] Update to match cython --- .../python/examples/ImuFactorExample.py | 5 +-- .../python/examples/ImuFactorExample2.py | 5 +-- .../python/examples/PlanarSLAMExample.py | 11 ++++--- .../python/examples/Pose2SLAMExample_g2o.py | 2 +- .../python/examples/Pose3SLAMExample_g2o.py | 4 +-- .../python/examples/PreintegrationExample.py | 2 +- python/gtsam_py/python/examples/README.md | 14 +++----- python/gtsam_py/python/examples/SFMExample.py | 32 ++++++++----------- python/gtsam_py/python/utils/plot.py | 14 ++++---- 9 files changed, 37 insertions(+), 52 deletions(-) diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py index e041afd87..1f8dfca07 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample.py +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -17,10 +17,7 @@ import math import gtsam import matplotlib.pyplot as plt import numpy as np -from gtsam import symbol_shorthand -B = symbol_shorthand.B -V = symbol_shorthand.V -X = symbol_shorthand.X +from gtsam.gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorExample2.py index 4f4b02f63..d5be69f47 100644 --- a/python/gtsam_py/python/examples/ImuFactorExample2.py +++ b/python/gtsam_py/python/examples/ImuFactorExample2.py @@ -17,10 +17,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorConstantBias, PriorFactorPose3, PriorFactorVector, Rot3, Values) -from gtsam import symbol_shorthand -B = symbol_shorthand.B -V = symbol_shorthand.V -X = symbol_shorthand.X +from gtsam.gtsam.symbol_shorthand import B, V, X from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 diff --git a/python/gtsam_py/python/examples/PlanarSLAMExample.py b/python/gtsam_py/python/examples/PlanarSLAMExample.py index 0baf2fa9a..ba0529479 100644 --- a/python/gtsam_py/python/examples/PlanarSLAMExample.py +++ b/python/gtsam_py/python/examples/PlanarSLAMExample.py @@ -16,6 +16,7 @@ from __future__ import print_function import numpy as np import gtsam +from gtsam.gtsam.symbol_shorthand import X, L # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) graph = gtsam.NonlinearFactorGraph() # Create the keys corresponding to unknown variables in the factor graph -X1 = gtsam.symbol('x', 1) -X2 = gtsam.symbol('x', 2) -X3 = gtsam.symbol('x', 3) -L1 = gtsam.symbol('l', 4) -L2 = gtsam.symbol('l', 5) +X1 = X(1) +X2 = X(2) +X3 = X(3) +L1 = L(4) +L2 = L(5) # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py index d27adfc2c..1ea7fc300 100644 --- a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py @@ -83,7 +83,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.extractPose2(result) + resultPoses = gtsam.utilities.extractPose2(result) for i in range(resultPoses.shape[0]): plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) plt.show() diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py index 20b02500e..c06a9d12f 100644 --- a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py @@ -44,7 +44,7 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, print("Adding prior to g2o file ") graphWithPrior = graph -firstKey = initial.keys().at(0) +firstKey = initial.keys()[0] graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() @@ -66,7 +66,7 @@ else: print ("Done!") if args.plot: - resultPoses = gtsam.allPose3s(result) + resultPoses = gtsam.utilities.allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show() diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py index e543e5d82..ee7f9cd8f 100644 --- a/python/gtsam_py/python/examples/PreintegrationExample.py +++ b/python/gtsam_py/python/examples/PreintegrationExample.py @@ -111,7 +111,7 @@ class PreintegrationExample(object): actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, 0.3) t = actualPose.translation() - self.maxDim = max([max(t), self.maxDim]) + self.maxDim = max([max(np.abs(t)), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) diff --git a/python/gtsam_py/python/examples/README.md b/python/gtsam_py/python/examples/README.md index ec4437d4c..46b87f079 100644 --- a/python/gtsam_py/python/examples/README.md +++ b/python/gtsam_py/python/examples/README.md @@ -1,18 +1,12 @@ -# THIS FILE IS OUTDATED! - -~~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('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 | +| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through Python | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through Python | | ImuFactorExample2 | X | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | @@ -26,7 +20,7 @@ | Pose2SLAMExample_g2o | X | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | -| Pose2SLAMExample_lago | lago not exposed through cython | +| Pose2SLAMExample_lago | lago not exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | | Pose3SLAMExample_changeKeys | | diff --git a/python/gtsam_py/python/examples/SFMExample.py b/python/gtsam_py/python/examples/SFMExample.py index 0c03a105d..f0c4c82ba 100644 --- a/python/gtsam_py/python/examples/SFMExample.py +++ b/python/gtsam_py/python/examples/SFMExample.py @@ -10,24 +10,21 @@ A structure-from-motion problem on a simulated dataset """ from __future__ import print_function +import gtsam import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand +L = symbol_shorthand.L +X = symbol_shorthand.X -import gtsam from gtsam.examples import SFMdata -from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, +from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, Point3, Pose3, - PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values) + NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, + Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values) from gtsam.utils import plot -def symbol(name: str, index: int) -> int: - """ helper for creating a symbol without explicitly casting 'name' from str to int """ - return gtsam.symbol(name, index) - - def main(): """ Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). @@ -74,23 +71,23 @@ def main(): # 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) + factor = PriorFactorPose3(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) + camera = PinholeCameraCal3_S2(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) + measurement, measurement_noise, X(i), 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) + factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) graph.print_('Factor Graph:\n') @@ -99,10 +96,10 @@ def main(): initial_estimate = Values() for i, pose in enumerate(poses): transformed_pose = pose.retract(0.1*np.random.randn(6,1)) - initial_estimate.insert(symbol('x', i), transformed_pose) + initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): - transformed_point = Point3(point.vector() + 0.1*np.random.randn(3)) - initial_estimate.insert(symbol('l', j), transformed_point) + transformed_point = point + 0.1*np.random.randn(3) + initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results @@ -121,6 +118,5 @@ def main(): plot.set_axes_equal(1) plt.show() - if __name__ == '__main__': main() diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index eef08ac88..ce602f506 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/python/gtsam_py/python/utils/plot.py @@ -221,9 +221,9 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None, keys = values.keys() # Plot points and covariance matrices - for i in range(keys.size()): + for i in range(len(keys)): try: - key = keys.at(i) + key = keys[i] point = values.atPoint3(key) if marginals is not None: covariance = marginals.marginalCovariance(key) @@ -319,19 +319,19 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, title (string): The title of the plot. axis_labels (iterable[string]): List of axis labels to set. """ - pose3Values = gtsam.utilities_allPose3s(values) + pose3Values = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) lastIndex = None - for i in range(keys.size()): - key = keys.at(i) + for i in range(len(keys)): + key = keys[i] try: pose = pose3Values.atPose3(key) except: print("Warning: no Pose3 at key: {0}".format(key)) if lastIndex is not None: - lastKey = keys.at(lastIndex) + lastKey = keys[lastIndex] try: lastPose = pose3Values.atPose3(lastKey) except: @@ -350,7 +350,7 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, # Draw final pose if lastIndex is not None: - lastKey = keys.at(lastIndex) + lastKey = keys[lastIndex] try: lastPose = pose3Values.atPose3(lastKey) if marginals: