diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 6cc8979f1..34a603be9 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -12,25 +12,18 @@ from __future__ import print_function import math +import gtsam import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand_B as B +from gtsam import symbol_shorthand_V as V +from gtsam import symbol_shorthand_X as X +from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D -import gtsam -from gtsam.utils.plot import plot_pose3 from PreintegrationExample import POSES_FIG, PreintegrationExample -BIAS_KEY = int(gtsam.symbol(ord('b'), 0)) - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol(ord('x'), key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol(ord('v'), key) +BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 80f573d12..0d98f08fe 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -8,27 +8,19 @@ from __future__ import print_function import math -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - import gtsam import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorConstantBias, PriorFactorPose3, PriorFactorVector, Rot3, Values) - - -def X(key): - """Create symbol for pose key.""" - return gtsam.symbol(ord('x'), key) - - -def V(key): - """Create symbol for velocity key.""" - return gtsam.symbol(ord('v'), key) +from gtsam import symbol_shorthand_B as B +from gtsam import symbol_shorthand_V as V +from gtsam import symbol_shorthand_X as X +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def vector3(x, y, z): @@ -115,7 +107,7 @@ def IMU_example(): newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors - biasKey = gtsam.symbol(ord('b'), 0) + biasKey = B(0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index fb038d043..c84f0f834 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) from __future__ import print_function -import numpy as np - import gtsam +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X # 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(ord('x'), 1) -X2 = gtsam.symbol(ord('x'), 2) -X3 = gtsam.symbol(ord('x'), 3) -L1 = gtsam.symbol(ord('l'), 4) -L2 = gtsam.symbol(ord('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/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 35ec4f66d..99bce00e2 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -1,7 +1,7 @@ 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))` +Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing. # Porting Progress diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index 09ee52da0..e02def2f9 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -13,6 +13,8 @@ from __future__ import print_function import gtsam import matplotlib.pyplot as plt import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X from gtsam.examples import SFMdata from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, @@ -22,11 +24,6 @@ from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, 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(ord(name), index) - - def main(): """ Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). @@ -73,7 +70,7 @@ 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 @@ -82,14 +79,14 @@ def main(): 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') @@ -98,10 +95,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) + initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py index 6e9b1320b..4e82d3778 100644 --- a/cython/gtsam/examples/SimpleRotation.py +++ b/cython/gtsam/examples/SimpleRotation.py @@ -10,8 +10,9 @@ This example will perform a relatively trivial optimization on a single variable with a single factor. """ -import numpy as np import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X def main(): @@ -33,7 +34,7 @@ def main(): 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) + key = X(1) factor = gtsam.PriorFactorRot2(key, prior, model) """ diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py index c0f4fbd94..49e6ca95c 100644 --- a/cython/gtsam/examples/VisualISAM2Example.py +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -13,23 +13,14 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python) from __future__ import print_function -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 - import gtsam import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X from gtsam.examples import SFMdata - - -def X(i): - """Create key for pose i.""" - return int(gtsam.symbol(ord('x'), i)) - - -def L(j): - """Create key for landmark j.""" - return int(gtsam.symbol(ord('l'), j)) +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def visual_ISAM2_plot(result): diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py index b7b83e2a1..5cc37867b 100644 --- a/cython/gtsam/examples/VisualISAMExample.py +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -19,11 +19,8 @@ from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, PinholeCameraCal3_S2, 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) +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X def main(): @@ -58,7 +55,7 @@ def main(): # 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) + factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth @@ -66,7 +63,7 @@ def main(): initial_xi = pose.compose(noise) # Add an initial guess for the current pose - initial_estimate.insert(symbol('x', i), initial_xi) + initial_estimate.insert(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 @@ -75,12 +72,12 @@ def main(): 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) + factor = PriorFactorPose3(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) + factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) # Add initial guesses to all observed landmarks @@ -88,7 +85,7 @@ def main(): 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)) + initial_estimate.insert(L(j), Point3(initial_lj)) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/cython/gtsam/tests/test_GaussianFactorGraph.py b/cython/gtsam/tests/test_GaussianFactorGraph.py index 2e7875af0..983825d8b 100644 --- a/cython/gtsam/tests/test_GaussianFactorGraph.py +++ b/cython/gtsam/tests/test_GaussianFactorGraph.py @@ -15,17 +15,18 @@ from __future__ import print_function import unittest import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X from gtsam.utils.test_case import GtsamTestCase -import numpy as np def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - x0 = gtsam.symbol(ord('x'), 0) - x1 = gtsam.symbol(ord('x'), 1) - x2 = gtsam.symbol(ord('x'), 2) + x0 = X(0) + x1 = X(1) + x2 = X(2) BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))