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

Interactive and shorthand symbols

release/4.3a0
Frank 2016-01-26 14:55:33 -08:00
parent ea3d72c66f
commit ac57680dee
1 changed files with 38 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
python/gtsam_examples/VisualISAM2Example.py
View File

@ -9,13 +9,17 @@ import gtsam
from gtsam_examples import SFMdata
import gtsam_utils
# shorthand symbols:
X = lambda i: gtsam.Symbol('x', i)
L = lambda j: gtsam.Symbol('l', j)
def visual_ISAM2_plot(poses, points, result):
# VisualISAMPlot plots current state of ISAM2 object
# Author: Ellon Paiva
# Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
# Declare an id for the figure
fignum = 0;
fignum = 0
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
@ -23,28 +27,28 @@ def visual_ISAM2_plot(poses, points, result):
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow
# gtsam.plot3DPoints(result, [], marginals);
gtsam_utils.plot3DPoints(fignum, result, 'rx');
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow
# gtsam.plot3DPoints(result, [], marginals)
gtsam_utils.plot3DPoints(fignum, result, 'rx')
# Plot cameras
M = 0;
while result.exists(int(gtsam.Symbol('x',M))):
ii = int(gtsam.Symbol('x',M));
pose_i = result.pose3_at(ii);
gtsam_utils.plotPose3(fignum, pose_i, 10);
M = 0
while result.exists(int(X(M))):
ii = int(X(M))
pose_i = result.pose3_at(ii)
gtsam_utils.plotPose3(fignum, pose_i, 10)
M = M + 1;
M = M + 1
# draw
ax.set_xlim3d(-40, 40)
ax.set_ylim3d(-40, 40)
ax.set_zlim3d(-40, 40)
plt.ion()
plt.show()
plt.draw()
plt.pause(1)
def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
@ -78,11 +82,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(gtsam.Symbol('x', i)), int(gtsam.Symbol('l', j)), K))
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(X(i)), int(L(j)), K))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
initialEstimate.insert(int(gtsam.Symbol('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(int(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
@ -91,16 +95,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(gtsam.Symbol('x', 0)), poses[0], poseNoise))
graph.push_back(gtsam.PriorFactorPose3(int(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(gtsam.Symbol('l', 0)), points[0], pointNoise)) # add directly to graph
graph.push_back(gtsam.PriorFactorPoint3(int(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(gtsam.Symbol('l', j)), point + gtsam.Point3(-0.25, 0.20, 0.15));
initialEstimate.insert(int(L(j)), point + gtsam.Point3(-0.25, 0.20, 0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initialEstimate)
@ -108,23 +112,23 @@ def visual_ISAM2_example():
# If accuracy is desired at the expense of time, update(*) can be called additional times
# to perform multiple optimizer iterations every step.
isam.update()
currentEstimate = isam.calculate_estimate();
currentEstimate = isam.calculate_estimate()
print("****************************************************")
print("Frame", i, ":")
for j in range(i + 1):
print( gtsam.Symbol('x',j) )
print( currentEstimate.pose3_at(int(gtsam.Symbol('x',j))) )
print(X(j), ":", currentEstimate.pose3_at(int(X(j))))
for j in range(len(points)):
print( gtsam.Symbol('l',j) )
print( currentEstimate.point3_at(int(gtsam.Symbol('l',j))) )
print(L(j), ":", currentEstimate.point3_at(int(L(j))))
visual_ISAM2_plot(poses, points, currentEstimate);
time.sleep(1)
visual_ISAM2_plot(poses, points, currentEstimate)
# Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
graph.resize(0)
initialEstimate.clear()
plt.ioff()
plt.show()
if __name__ == '__main__':
visual_ISAM2_example()