From 69a53f8e00191aac62d9ac1db5c8084de08bb2db Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 27 Jan 2016 14:16:09 -0800 Subject: [PATCH] simplify keys --- python/gtsam_examples/VisualISAM2Example.py | 28 ++++++++++----------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/python/gtsam_examples/VisualISAM2Example.py b/python/gtsam_examples/VisualISAM2Example.py index a0e40a146..29a8180ad 100644 --- a/python/gtsam_examples/VisualISAM2Example.py +++ b/python/gtsam_examples/VisualISAM2Example.py @@ -10,8 +10,8 @@ from gtsam_examples import SFMdata import gtsam_utils # shorthand symbols: -X = lambda i: gtsam.Symbol('x', i) -L = lambda j: gtsam.Symbol('l', j) +X = lambda i: int(gtsam.Symbol('x', i)) +L = lambda j: int(gtsam.Symbol('l', j)) def visual_ISAM2_plot(poses, points, result): # VisualISAMPlot plots current state of ISAM2 object @@ -32,13 +32,11 @@ def visual_ISAM2_plot(poses, points, result): gtsam_utils.plot3DPoints(fignum, result, 'rx') # Plot cameras - M = 0 - while result.exists(int(X(M))): - ii = int(X(M)) - pose_i = result.pose3_at(ii) + i = 0 + while result.exists(X(i)): + pose_i = result.pose3_at(X(i)) gtsam_utils.plotPose3(fignum, pose_i, 10) - - M = M + 1 + i += 1 # draw ax.set_xlim3d(-40, 40) @@ -82,11 +80,11 @@ def visual_ISAM2_example(): for j, point in enumerate(points): camera = gtsam.PinholeCameraCal3_S2(pose, K) measurement = camera.project(point) - graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(X(i)), int(L(j)), K)) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K)) # Add an initial guess for the current pose # Intentionally initialize the variables off from the ground truth - initialEstimate.insert(int(X(i)), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) # 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 @@ -95,16 +93,16 @@ def visual_ISAM2_example(): if(i == 0): # Add a prior on pose x0 poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(gtsam.PriorFactorPose3(int(X(0)), poses[0], poseNoise)) + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise)) # Add a prior on landmark l0 pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - graph.push_back(gtsam.PriorFactorPoint3(int(L(0)), points[0], pointNoise)) # add directly to graph + graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise)) # add directly to graph # Add initial guesses to all observed landmarks # Intentionally initialize the variables off from the ground truth for j, point in enumerate(points): - initialEstimate.insert(int(L(j)), point + gtsam.Point3(-0.25, 0.20, 0.15)) + initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15)) else: # Update iSAM with the new factors isam.update(graph, initialEstimate) @@ -116,10 +114,10 @@ def visual_ISAM2_example(): print("****************************************************") print("Frame", i, ":") for j in range(i + 1): - print(X(j), ":", currentEstimate.pose3_at(int(X(j)))) + print(X(j), ":", currentEstimate.pose3_at(X(j))) for j in range(len(points)): - print(L(j), ":", currentEstimate.point3_at(int(L(j)))) + print(L(j), ":", currentEstimate.point3_at(L(j))) visual_ISAM2_plot(poses, points, currentEstimate)