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

72
python/gtsam_examples/VisualISAM2Example.py
View File

@ -3,19 +3,23 @@ from __future__ import print_function
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d import Axes3D
import numpy as np import numpy as np
import time # for sleep() import time # for sleep()
import gtsam import gtsam
from gtsam_examples import SFMdata from gtsam_examples import SFMdata
import gtsam_utils 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): def visual_ISAM2_plot(poses, points, result):
# VisualISAMPlot plots current state of ISAM2 object # VisualISAMPlot plots current state of ISAM2 object
# Author: Ellon Paiva # Author: Ellon Paiva
# Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
# Declare an id for the figure # Declare an id for the figure
fignum = 0; fignum = 0
fig = plt.figure(fignum) fig = plt.figure(fignum)
ax = fig.gca(projection='3d') ax = fig.gca(projection='3d')
@ -23,33 +27,33 @@ def visual_ISAM2_plot(poses, points, result):
# Plot points # Plot points
# Can't use data because current frame might not see all points # Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow
# gtsam.plot3DPoints(result, [], marginals); # gtsam.plot3DPoints(result, [], marginals)
gtsam_utils.plot3DPoints(fignum, result, 'rx'); gtsam_utils.plot3DPoints(fignum, result, 'rx')
# Plot cameras # Plot cameras
M = 0; M = 0
while result.exists(int(gtsam.Symbol('x',M))): while result.exists(int(X(M))):
ii = int(gtsam.Symbol('x',M)); ii = int(X(M))
pose_i = result.pose3_at(ii); pose_i = result.pose3_at(ii)
gtsam_utils.plotPose3(fignum, pose_i, 10); gtsam_utils.plotPose3(fignum, pose_i, 10)
M = M + 1; M = M + 1
# draw # draw
ax.set_xlim3d(-40, 40) ax.set_xlim3d(-40, 40)
ax.set_ylim3d(-40, 40) ax.set_ylim3d(-40, 40)
ax.set_zlim3d(-40, 40) ax.set_zlim3d(-40, 40)
plt.ion() plt.pause(1)
plt.show()
plt.draw()
def visual_ISAM2_example(): def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters # Define the camera calibration parameters
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model # Define the camera observation noise model
measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks # Create the set of ground-truth landmarks
points = SFMdata.createPoints() points = SFMdata.createPoints()
@ -78,29 +82,29 @@ 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(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 # 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(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 # 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
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM. # adding it to iSAM.
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(gtsam.Symbol('x', 0)), poses[0], poseNoise)) graph.push_back(gtsam.PriorFactorPose3(int(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(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 # 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(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: else:
# Update iSAM with the new factors # Update iSAM with the new factors
isam.update(graph, initialEstimate) 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 # If accuracy is desired at the expense of time, update(*) can be called additional times
# to perform multiple optimizer iterations every step. # to perform multiple optimizer iterations every step.
isam.update() isam.update()
currentEstimate = isam.calculate_estimate(); currentEstimate = isam.calculate_estimate()
print( "****************************************************" ) print("****************************************************")
print( "Frame", i, ":" ) print("Frame", i, ":")
for j in range(i+1): for j in range(i + 1):
print( gtsam.Symbol('x',j) ) print(X(j), ":", currentEstimate.pose3_at(int(X(j))))
print( currentEstimate.pose3_at(int(gtsam.Symbol('x',j))) )
for j in range(len(points)): for j in range(len(points)):
print( gtsam.Symbol('l',j) ) print(L(j), ":", currentEstimate.point3_at(int(L(j))))
print( currentEstimate.point3_at(int(gtsam.Symbol('l',j))) )
visual_ISAM2_plot(poses, points, currentEstimate); visual_ISAM2_plot(poses, points, currentEstimate)
time.sleep(1)
# Clear the factor graph and values for the next iteration # Clear the factor graph and values for the next iteration
graph.resize(0); graph.resize(0)
initialEstimate.clear(); initialEstimate.clear()
plt.ioff()
plt.show()
if __name__ == '__main__': if __name__ == '__main__':
visual_ISAM2_example() visual_ISAM2_example()