diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 555808d5a..e6fd8c9c8 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,16 +1,21 @@ from .gtsam import * + def _init(): + """This function is to add shims for the long-gone Point2 and Point3 types""" + import numpy as np global Point2 # export function def Point2(x=0, y=0): + """Shim for the deleted Point2 type.""" return np.array([x, y], dtype=float) global Point3 # export function def Point3(x=0, y=0, z=0): + """Shim for the deleted Point3 type.""" return np.array([x, y, z], dtype=float) # for interactive debugging diff --git a/python/gtsam/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py index e0999b25b..98f6218ea 100644 --- a/python/gtsam/examples/ImuFactorISAM2Example.py +++ b/python/gtsam/examples/ImuFactorISAM2Example.py @@ -1,6 +1,6 @@ """ iSAM2 example with ImuFactor. -Author: Frank Dellaert, Varun Agrawal +Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal """ # pylint: disable=invalid-name, E1101 diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 46b87f079..e998e4dcd 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -5,8 +5,8 @@ | CameraResectioning | | | CreateSFMExampleData | | | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | -| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through Python | -| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through Python | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | | ImuFactorExample2 | X | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | @@ -20,7 +20,7 @@ | Pose2SLAMExample_g2o | X | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | -| Pose2SLAMExample_lago | lago not exposed through Python | +| Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | | Pose3SLAMExample_changeKeys | | @@ -42,11 +42,11 @@ | StereoVOExample | | | StereoVOExample_large | | | TimeTBB | | -| UGM_chain | discrete functionality not exposed | -| UGM_small | discrete functionality not exposed | +| UGM_chain | discrete functionality not yet exposed | +| UGM_small | discrete functionality not yet exposed | | VisualISAM2Example | X | | VisualISAMExample | X | Extra Examples (with no C++ equivalent) - PlanarManipulatorExample -- SFMData +- SFMData \ No newline at end of file diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index 7d562c9b4..f99d3f3e6 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -19,12 +19,7 @@ from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, NonlinearISAM, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, PinholeCameraCal3_S2, Values, Point3) - - -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) - +from gtsam.symbol_shorthand import X, L def main(): """ @@ -60,7 +55,7 @@ def main(): for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2( - measurement, camera_noise, symbol('x', i), symbol('l', j), K) + measurement, camera_noise, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth @@ -69,7 +64,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 @@ -79,12 +74,12 @@ def main(): # 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 @@ -92,7 +87,7 @@ def main(): for j, point in enumerate(points): # Intentionally initialize the variables off from the ground truth initial_lj = points[j] + noise - initial_estimate.insert(symbol('l', j), initial_lj) + initial_estimate.insert(L(j), initial_lj) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) diff --git a/python/gtsam/tests/test_SFMExample.py b/python/gtsam/tests/test_SFMExample.py index 5e787f5ae..47a3cbe3e 100644 --- a/python/gtsam/tests/test_SFMExample.py +++ b/python/gtsam/tests/test_SFMExample.py @@ -17,7 +17,7 @@ import gtsam.utils.visual_data_generator as generator from gtsam import symbol from gtsam.noiseModel import Isotropic, Diagonal from gtsam.utils.test_case import GtsamTestCase - +from gtsam.symbol_shorthand import X, P class TestSFMExample(GtsamTestCase): @@ -41,23 +41,23 @@ class TestSFMExample(GtsamTestCase): j = data.J[i][k] graph.add(gtsam.GenericProjectionFactorCal3_S2( data.Z[i][k], measurementNoise, - symbol('x', i), symbol('p', j), data.K)) + X(i), P(j), data.K)) posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) - graph.add(gtsam.PriorFactorPose3(symbol('x', 0), + graph.add(gtsam.PriorFactorPose3(X(0), truth.cameras[0].pose(), posePriorNoise)) pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) - graph.add(gtsam.PriorFactorPoint3(symbol('p', 0), + graph.add(gtsam.PriorFactorPoint3(P(0), truth.points[0], pointPriorNoise)) # Initial estimate initialEstimate = gtsam.Values() for i in range(len(truth.cameras)): pose_i = truth.cameras[i].pose() - initialEstimate.insert(symbol('x', i), pose_i) + initialEstimate.insert(X(i), pose_i) for j in range(len(truth.points)): point_j = truth.points[j] - initialEstimate.insert(symbol('p', j), point_j) + initialEstimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) @@ -67,16 +67,16 @@ class TestSFMExample(GtsamTestCase): # Marginalization marginals = gtsam.Marginals(graph, result) - marginals.marginalCovariance(symbol('p', 0)) - marginals.marginalCovariance(symbol('x', 0)) + marginals.marginalCovariance(P(0)) + marginals.marginalCovariance(X(0)) # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): - pose_i = result.atPose3(symbol('x', i)) + pose_i = result.atPose3(X(i)) self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): - point_j = result.atPoint3(symbol('p', j)) + point_j = result.atPoint3(P(j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 90f5fe5e7..8b34ad2bf 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -109,7 +109,7 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() - origin = np.array(t) + origin = t # draw the camera axes x_axis = origin + gRp[:, 0] * axis_length @@ -221,9 +221,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None, keys = values.keys() # Plot points and covariance matrices - for i in range(len(keys)): + for key in keys: try: - key = keys[i] point = values.atPoint3(key) if marginals is not None: covariance = marginals.marginalCovariance(key) @@ -321,17 +320,15 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, """ pose3Values = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) - lastIndex = None + lastKey = None - for i in range(len(keys)): - key = keys[i] + for key in keys: try: pose = pose3Values.atPose3(key) except: print("Warning: no Pose3 at key: {0}".format(key)) - if lastIndex is not None: - lastKey = keys[lastIndex] + if lastKey is not None: try: lastPose = pose3Values.atPose3(lastKey) except: @@ -346,11 +343,10 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, fig = plot_pose3(fignum, lastPose, P=covariance, axis_length=scale, axis_labels=axis_labels) - lastIndex = i + lastKey = key # Draw final pose - if lastIndex is not None: - lastKey = keys[lastIndex] + if lastKey is not None: try: lastPose = pose3Values.atPose3(lastKey) if marginals: @@ -390,10 +386,9 @@ def plot_incremental_trajectory(fignum, values, start=0, pose3Values = gtsam.utilities.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) - for i in range(start, len(keys)): - key = keys[i] + for key in keys[start:]: if values.exists(key): - pose_i = values.atPose3(keys[i]) + pose_i = values.atPose3(key) plot_pose3(fignum, pose_i, scale) # Update the plot space to encompass all plotted points