2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 visual_isam unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta & Varun Agrawal (Python)
20 from gtsam
import symbol
24 """Test class for ISAM2 with visual landmarks."""
28 options = generator.Options()
29 options.triangle =
False
30 options.nrCameras = 20
34 isamOptions = visual_isam.Options()
35 isamOptions.hardConstraint =
False
36 isamOptions.pointPriors =
False
37 isamOptions.batchInitialization =
True
38 isamOptions.reorderInterval = 10
39 isamOptions.alwaysRelinearize =
False
43 self.data, self.
truth = generator.generate_data(options)
46 """Test to see if ISAM works as expected for a simple visual SLAM example."""
48 isam, result, nextPose = visual_isam.initialize(
52 for currentPose
in range(nextPose, self.
options.nrCameras):
53 isam, result = visual_isam.step(self.data, isam, result,
54 self.
truth, currentPose)
56 for i, true_camera
in enumerate(self.
truth.cameras):
57 pose_i = result.atPose3(
symbol(
'x', i))
60 for j, expected_point
in enumerate(self.
truth.points):
61 point_j = result.atPoint3(
symbol(
'l', j))
65 """Test for isam2 error() method."""
67 isam, result, nextPose = visual_isam.initialize(
71 for currentPose
in range(nextPose, self.
options.nrCameras):
72 isam, result = visual_isam.step(self.data, isam, result,
73 self.
truth, currentPose)
77 estimate = isam.calculateBestEstimate()
79 for key
in estimate.keys():
83 v = estimate.atPoint3(key)
87 self.assertAlmostEqual(isam.error(values), 34212421.14732)
91 Test for full version of ISAM2::update method
94 isam, result, nextPose = visual_isam.initialize(
97 remove_factor_indices = []
101 isamArgs = (remove_factor_indices, constrained_keys, no_relin_keys,
102 extra_reelim_keys,
False)
105 for currentPose
in range(nextPose, self.
options.nrCameras):
106 isam, result = visual_isam.step(self.data, isam, result,
107 self.
truth, currentPose, isamArgs)
110 pose_i = result.atPose3(
symbol(
'x', i))
114 point_j = result.atPoint3(
symbol(
'l', j))
118 if __name__ ==
"__main__":