12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

simplify keys

release/4.3a0
Frank 2016-01-27 14:16:09 -08:00
parent 02e2b37b08
commit 69a53f8e00
1 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
python/gtsam_examples/VisualISAM2Example.py
View File

@ -10,8 +10,8 @@ from gtsam_examples import SFMdata
import gtsam_utils import gtsam_utils
# shorthand symbols: # shorthand symbols:
X = lambda i: gtsam.Symbol('x', i) X = lambda i: int(gtsam.Symbol('x', i))
L = lambda j: gtsam.Symbol('l', j) L = lambda j: int(gtsam.Symbol('l', j))
def visual_ISAM2_plot(poses, points, result): def visual_ISAM2_plot(poses, points, result):
# VisualISAMPlot plots current state of ISAM2 object # VisualISAMPlot plots current state of ISAM2 object
@ -32,13 +32,11 @@ def visual_ISAM2_plot(poses, points, result):
gtsam_utils.plot3DPoints(fignum, result, 'rx') gtsam_utils.plot3DPoints(fignum, result, 'rx')
# Plot cameras # Plot cameras
M = 0 i = 0
while result.exists(int(X(M))): while result.exists(X(i)):
ii = int(X(M)) pose_i = result.pose3_at(X(i))
pose_i = result.pose3_at(ii)
gtsam_utils.plotPose3(fignum, pose_i, 10) gtsam_utils.plotPose3(fignum, pose_i, 10)
i += 1
M = M + 1
# draw # draw
ax.set_xlim3d(-40, 40) ax.set_xlim3d(-40, 40)
@ -82,11 +80,11 @@ def visual_ISAM2_example():
for j, point in enumerate(points): for j, point in enumerate(points):
camera = gtsam.PinholeCameraCal3_S2(pose, K) camera = gtsam.PinholeCameraCal3_S2(pose, K)
measurement = camera.project(point) 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 # Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth # 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 # 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 # and a prior on the first landmark to set the scale
@ -95,16 +93,16 @@ def visual_ISAM2_example():
if(i == 0): if(i == 0):
# Add a prior on pose x0 # 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 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 # Add a prior on landmark l0
pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) 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 # Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth # Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points): 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: else:
# Update iSAM with the new factors # Update iSAM with the new factors
isam.update(graph, initialEstimate) isam.update(graph, initialEstimate)
@ -116,10 +114,10 @@ def visual_ISAM2_example():
print("****************************************************") print("****************************************************")
print("Frame", i, ":") print("Frame", i, ":")
for j in range(i + 1): 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)): 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) visual_ISAM2_plot(poses, points, currentEstimate)