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
# 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)