test_TranslationRecovery.py
Go to the documentation of this file.
1 import unittest
2 
3 import gtsam
4 import numpy as np
5 
6 
8  """ Returns example pose values of 3 points A, B and C in the world frame """
9  T = []
10  T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
11  T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
12  T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
13 
14  data = gtsam.Values()
15  for i, t in enumerate(T):
16  data.insert(i, gtsam.Pose3(gtsam.Rot3(), t))
17  return data
18 
19 
20 def SimulateMeasurements(gt_poses, graph_edges):
21  """ Returns binary measurements for the points in the given edges."""
22  measurements = []
23  for edge in graph_edges:
24  Ta = gt_poses.atPose3(edge[0]).translation()
25  Tb = gt_poses.atPose3(edge[1]).translation()
26  measurements.append(gtsam.BinaryMeasurementUnit3( \
27  edge[0], edge[1], gtsam.Unit3(Tb - Ta), \
29  return measurements
30 
31 
32 class TestTranslationRecovery(unittest.TestCase):
33  """ Tests for the translation recovery class."""
34 
35  def test_constructor(self):
36  """Construct from binary measurements."""
37  algorithm = gtsam.TranslationRecovery()
38  self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
39  algorithm_params = gtsam.TranslationRecovery(
41  self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
42 
43  def test_run(self):
44  """Test selected Translation Recovery methods."""
45  gt_poses = ExampleValues()
46  measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
47 
48  # Set verbosity to Silent for tests
50  lmParams.setVerbosityLM("silent")
51 
52  algorithm = gtsam.TranslationRecovery(lmParams)
53  scale = 2.0
54  result = algorithm.run(measurements, scale)
55 
56  w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(
57  0).translation()
58  w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(
59  0).translation()
60  w_aTc_expected = w_aTc * scale / np.linalg.norm(w_aTb)
61  w_aTb_expected = w_aTb * scale / np.linalg.norm(w_aTb)
62 
63  np.testing.assert_array_almost_equal(result.atPoint3(0),
64  np.array([0, 0, 0]), 6,
65  "Origin result is incorrect.")
66  np.testing.assert_array_almost_equal(result.atPoint3(1),
67  w_aTb_expected, 6,
68  "Point B result is incorrect.")
69  np.testing.assert_array_almost_equal(result.atPoint3(2),
70  w_aTc_expected, 6,
71  "Point C result is incorrect.")
72 
73 
74 if __name__ == "__main__":
75  unittest.main()
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
def SimulateMeasurements(gt_poses, graph_edges)
Vector3 Point3
Definition: Point3.h:38
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:46