test_VisualISAMExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 visual_isam unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta & Varun Agrawal (Python)
10 """
11 # pylint: disable=maybe-no-member,invalid-name
12 
13 import unittest
14 
15 import gtsam.utils.visual_data_generator as generator
16 import gtsam.utils.visual_isam as visual_isam
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 import gtsam
20 from gtsam import symbol
21 
22 
24  """Test class for ISAM2 with visual landmarks."""
25 
26  def setUp(self):
27  # Data Options
28  options = generator.Options()
29  options.triangle = False
30  options.nrCameras = 20
31  self.options = options
32 
33  # iSAM Options
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
40  self.isamOptions = isamOptions
41 
42  # Generate data
43  self.data, self.truth = generator.generate_data(options)
44 
46  """Test to see if ISAM works as expected for a simple visual SLAM example."""
47  # Initialize iSAM with the first pose and points
48  isam, result, nextPose = visual_isam.initialize(
49  self.data, self.truth, self.isamOptions)
50 
51  # Main loop for iSAM: stepping through all poses
52  for currentPose in range(nextPose, self.options.nrCameras):
53  isam, result = visual_isam.step(self.data, isam, result,
54  self.truth, currentPose)
55 
56  for i, true_camera in enumerate(self.truth.cameras):
57  pose_i = result.atPose3(symbol('x', i))
58  self.gtsamAssertEquals(pose_i, true_camera.pose(), 1e-5)
59 
60  for j, expected_point in enumerate(self.truth.points):
61  point_j = result.atPoint3(symbol('l', j))
62  self.gtsamAssertEquals(point_j, expected_point, 1e-5)
63 
64  def test_isam2_error(self):
65  """Test for isam2 error() method."""
66  # Initialize iSAM with the first pose and points
67  isam, result, nextPose = visual_isam.initialize(
68  self.data, self.truth, self.isamOptions)
69 
70  # Main loop for iSAM: stepping through all poses
71  for currentPose in range(nextPose, self.options.nrCameras):
72  isam, result = visual_isam.step(self.data, isam, result,
73  self.truth, currentPose)
74 
75  values = gtsam.VectorValues()
76 
77  estimate = isam.calculateBestEstimate()
78 
79  for key in estimate.keys():
80  try:
81  v = gtsam.Pose3.Logmap(estimate.atPose3(key))
82  except RuntimeError:
83  v = estimate.atPoint3(key)
84 
85  values.insert(key, v)
86 
87  self.assertAlmostEqual(isam.error(values), 34212421.14732)
88 
89  def test_isam2_update(self):
90  """
91  Test for full version of ISAM2::update method
92  """
93  # Initialize iSAM with the first pose and points
94  isam, result, nextPose = visual_isam.initialize(
95  self.data, self.truth, self.isamOptions)
96 
97  remove_factor_indices = []
98  constrained_keys = gtsam.KeyGroupMap()
99  no_relin_keys = gtsam.KeyList()
100  extra_reelim_keys = gtsam.KeyList()
101  isamArgs = (remove_factor_indices, constrained_keys, no_relin_keys,
102  extra_reelim_keys, False)
103 
104  # Main loop for iSAM: stepping through all poses
105  for currentPose in range(nextPose, self.options.nrCameras):
106  isam, result = visual_isam.step(self.data, isam, result,
107  self.truth, currentPose, isamArgs)
108 
109  for i in range(len(self.truth.cameras)):
110  pose_i = result.atPose3(symbol('x', i))
111  self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5)
112 
113  for j in range(len(self.truth.points)):
114  point_j = result.atPoint3(symbol('l', j))
115  self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5)
116 
117 
118 if __name__ == "__main__":
119  unittest.main()
test_VisualISAMExample.TestVisualISAMExample
Definition: test_VisualISAMExample.py:23
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::Pose3::Logmap
static Vector6 Logmap(const Pose3 &pose, OptionalJacobian< 6, 6 > Hpose={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3.cpp:188
gtsam::FastMap
Definition: FastMap.h:39
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::VectorValues
Definition: VectorValues.h:74
test_VisualISAMExample.TestVisualISAMExample.test_VisualISAMExample
def test_VisualISAMExample(self)
Definition: test_VisualISAMExample.py:45
gtsam::utils.visual_isam
Definition: visual_isam.py:1
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
gtsam::utils.test_case
Definition: test_case.py:1
test_VisualISAMExample.TestVisualISAMExample.truth
truth
Definition: test_VisualISAMExample.py:43
gtsam::FastList
Definition: FastList.h:43
test_VisualISAMExample.TestVisualISAMExample.options
options
Definition: test_VisualISAMExample.py:31
test_VisualISAMExample.TestVisualISAMExample.isamOptions
isamOptions
Definition: test_VisualISAMExample.py:40
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2446
gtsam::utils.visual_data_generator
Definition: visual_data_generator.py:1
test_VisualISAMExample.TestVisualISAMExample.test_isam2_error
def test_isam2_error(self)
Definition: test_VisualISAMExample.py:64
test_VisualISAMExample.TestVisualISAMExample.setUp
def setUp(self)
Definition: test_VisualISAMExample.py:26
test_VisualISAMExample.TestVisualISAMExample.test_isam2_update
def test_isam2_update(self)
Definition: test_VisualISAMExample.py:89


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:33