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

Cleaned up example

release/4.3a0
Frank Dellaert 2016-01-25 00:57:35 -08:00
parent 6ee3e42d27
commit 312b8f5da0
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
python/gtsam_examples/VisualISAM2Example.py
View File

@ -1,11 +1,14 @@
from __future__ import print_function from __future__ import print_function
import gtsam
from gtsam_examples.SFMdata import *
from gtsam_utils import *
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 time # for sleep() import time # for sleep()
import gtsam
from gtsam_examples import SFMdata
import gtsam_utils
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
@ -22,14 +25,14 @@ def visual_ISAM2_plot(poses, points, result):
# 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);
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(gtsam.Symbol('x',M))):
ii = int(gtsam.Symbol('x',M)); ii = int(gtsam.Symbol('x',M));
pose_i = result.pose3_at(ii); pose_i = result.pose3_at(ii);
plotPose3(fignum, pose_i, 10); gtsam_utils.plotPose3(fignum, pose_i, 10);
M = M + 1; M = M + 1;
@ -49,10 +52,10 @@ def visual_ISAM2_example():
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 = createPoints() # from SFMdata points = SFMdata.createPoints()
# Create the set of ground-truth poses # Create the set of ground-truth poses
poses = createPoses() # from SFMdata poses = SFMdata.createPoses()
# Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
# and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter