test_Utilities.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 Utilities unit tests.
9 Author: Varun Agrawal
10 """
11 # pylint: disable=invalid-name, E1101, E0611
12 import unittest
13 
14 import numpy as np
15 from gtsam.utils.test_case import GtsamTestCase
16 
17 import gtsam
18 
19 
21  """Test various GTSAM utilities."""
22 
23  def test_createKeyList(self):
24  """Test createKeyList."""
25  I = [0, 1, 2]
27  self.assertEqual(kl.size(), 3)
28 
30  self.assertEqual(kl.size(), 3)
31 
33  """Tests for KeyList iteration"""
34  I = [0, 1, 2]
36 
37  self.assertEqual(len(kl), len(I))
38 
39  for i, key in enumerate(kl):
40  self.assertTrue(key in kl)
41  self.assertEqual(I[i], key)
42 
44  """Test createKeyVector."""
45  I = [0, 1, 2]
47  self.assertEqual(len(kl), 3)
48 
50  self.assertEqual(len(kl), 3)
51 
53  """Tests for KeyVector iteration"""
54  I = [0, 1, 2]
56 
57  self.assertEqual(len(kv), len(I))
58 
59  for i, key in enumerate(kv):
60  self.assertTrue(key in kv)
61  self.assertEqual(I[i], key)
62 
63  def test_createKeySet(self):
64  """Test createKeySet."""
65  I = [0, 1, 2]
67  self.assertEqual(kl.size(), 3)
68 
69  kl = gtsam.utilities.createKeySet("s", I)
70  self.assertEqual(kl.size(), 3)
71 
73  """Tests for KeySet iteration"""
74  I = [0, 1, 2]
76 
77  self.assertEqual(len(ks), len(I))
78 
79  for i, key in enumerate(ks):
80  self.assertTrue(key in ks)
81  self.assertEqual(I[i], key)
82 
83  def test_extractPoint2(self):
84  """Test extractPoint2."""
85  initial = gtsam.Values()
86  point2 = gtsam.Point2(0.0, 0.1)
87  initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
88  initial.insert(2, point2)
89  np.testing.assert_equal(gtsam.utilities.extractPoint2(initial),
90  point2.reshape(-1, 2))
91 
92  def test_extractPoint3(self):
93  """Test extractPoint3."""
94  initial = gtsam.Values()
95  point3 = gtsam.Point3(0.0, 0.1, 0.0)
96  initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
97  initial.insert(2, point3)
98  np.testing.assert_equal(gtsam.utilities.extractPoint3(initial),
99  point3.reshape(-1, 3))
100 
101  def test_allPose2s(self):
102  """Test allPose2s."""
103  initial = gtsam.Values()
104  initial.insert(0, gtsam.Pose2())
105  initial.insert(1, gtsam.Pose2(1, 1, 1))
106  initial.insert(2, gtsam.Point2(1, 1))
107  initial.insert(3, gtsam.Point3(1, 2, 3))
108  result = gtsam.utilities.allPose2s(initial)
109  self.assertEqual(result.size(), 2)
110 
111  def test_extractPose2(self):
112  """Test extractPose2."""
113  initial = gtsam.Values()
114  pose2 = np.asarray((0.0, 0.1, 0.1))
115 
116  initial.insert(1, gtsam.Pose2(*pose2))
117  initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0))
118  np.testing.assert_allclose(gtsam.utilities.extractPose2(initial),
119  pose2.reshape(-1, 3))
120 
121  def test_allPose3s(self):
122  """Test allPose3s."""
123  initial = gtsam.Values()
124  initial.insert(0, gtsam.Pose3())
125  initial.insert(2, gtsam.Point2(1, 1))
126  initial.insert(1, gtsam.Pose3())
127  initial.insert(3, gtsam.Point3(1, 2, 3))
128  result = gtsam.utilities.allPose3s(initial)
129  self.assertEqual(result.size(), 2)
130 
131  def test_extractPose3(self):
132  """Test extractPose3."""
133  initial = gtsam.Values()
134  pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.])
135  initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
136  initial.insert(2, gtsam.Pose3())
137  np.testing.assert_allclose(gtsam.utilities.extractPose3(initial),
138  pose3.reshape(-1, 12))
139 
141  """Test perturbPoint2."""
142  values = gtsam.Values()
143  values.insert(0, gtsam.Pose3())
144  values.insert(1, gtsam.Point2(1, 1))
145  gtsam.utilities.perturbPoint2(values, 1.0)
146  self.assertTrue(
147  not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1)))
148 
149  def test_perturbPose2(self):
150  """Test perturbPose2."""
151  values = gtsam.Values()
152  values.insert(0, gtsam.Pose2())
153  values.insert(1, gtsam.Point2(1, 1))
154  gtsam.utilities.perturbPose2(values, 1, 1)
155  self.assertTrue(values.atPose2(0) != gtsam.Pose2())
156 
158  """Test perturbPoint3."""
159  values = gtsam.Values()
160  point3 = gtsam.Point3(0, 0, 0)
161  values.insert(0, gtsam.Pose2())
162  values.insert(1, point3)
164  self.assertTrue(not np.allclose(values.atPoint3(1), point3))
165 
167  """Test insertBackprojections."""
168  values = gtsam.Values()
171  values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
172  10)
173  np.testing.assert_allclose(values.atPoint3(0),
174  gtsam.Point3(200, 200, 10))
175 
177  """Test insertProjectionFactors."""
180  graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
182  self.assertEqual(graph.size(), 2)
183 
186  graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
188  gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)))
189  self.assertEqual(graph.size(), 2)
190 
192  """Test reprojectionErrors."""
193  pixels = np.asarray([[20, 30], [20, 30]])
194  I = [1, 2]
195  K = gtsam.Cal3_S2()
198  graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K)
199  values = gtsam.Values()
200  values.insert(0, gtsam.Pose3())
202  gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10)
203  errors = gtsam.utilities.reprojectionErrors(graph, values)
204  np.testing.assert_allclose(errors, np.zeros((2, 2)))
205 
206  def test_localToWorld(self):
207  """Test localToWorld."""
208  local = gtsam.Values()
209  local.insert(0, gtsam.Point2(10, 10))
210  local.insert(1, gtsam.Pose2(6, 11, 0.0))
211  base = gtsam.Pose2(1, 0, 0)
212  world = gtsam.utilities.localToWorld(local, base)
213 
214  expected_point2 = gtsam.Point2(11, 10)
215  expected_pose2 = gtsam.Pose2(7, 11, 0)
216  np.testing.assert_allclose(world.atPoint2(0), expected_point2)
217  np.testing.assert_allclose(
218  world.atPose2(1).matrix(), expected_pose2.matrix())
219 
220  user_keys = [1]
221  world = gtsam.utilities.localToWorld(local, base, user_keys)
222  np.testing.assert_allclose(
223  world.atPose2(1).matrix(), expected_pose2.matrix())
224 
225  # Raise error since 0 is not in user_keys
226  self.assertRaises(RuntimeError, world.atPoint2, 0)
227 
228 
229 if __name__ == "__main__":
230  unittest.main()
test_Utilities.TestUtilites.test_extractPoint2
def test_extractPoint2(self)
Definition: test_Utilities.py:83
gtsam::utilities::allPose3s
Values allPose3s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:150
gtsam::utilities::allPose2s
Values allPose2s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:132
test_Utilities.TestUtilites.test_extractPose2
def test_extractPose2(self)
Definition: test_Utilities.py:111
test_Utilities.TestUtilites.test_KeySet_iteration
def test_KeySet_iteration(self)
Definition: test_Utilities.py:72
test_Utilities.TestUtilites.test_KeyVector_iteration
def test_KeyVector_iteration(self)
Definition: test_Utilities.py:52
test_Utilities.TestUtilites.test_perturbPoint2
def test_perturbPoint2(self)
Definition: test_Utilities.py:140
test_Utilities.TestUtilites.test_perturbPose2
def test_perturbPose2(self)
Definition: test_Utilities.py:149
test_Utilities.TestUtilites.test_perturbPoint3
def test_perturbPoint3(self)
Definition: test_Utilities.py:157
gtsam::utilities::extractPose2
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
Definition: nonlinear/utilities.h:140
test_Utilities.TestUtilites.test_createKeyList
def test_createKeyList(self)
Definition: test_Utilities.py:23
test_Utilities.TestUtilites.test_insertProjectionFactors
def test_insertProjectionFactors(self)
Definition: test_Utilities.py:176
gtsam::utilities::localToWorld
Values localToWorld(const Values &local, const Pose2 &base, const KeyVector user_keys=KeyVector())
Convert from local to world coordinates.
Definition: nonlinear/utilities.h:315
gtsam::utilities::extractPoint3
Matrix extractPoint3(const Values &values)
Extract all Point3 values into a single matrix [x y z].
Definition: nonlinear/utilities.h:112
test_Utilities.TestUtilites.test_extractPoint3
def test_extractPoint3(self)
Definition: test_Utilities.py:92
test_Utilities.TestUtilites.test_insertBackprojections
def test_insertBackprojections(self)
Definition: test_Utilities.py:166
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
test_Utilities.TestUtilites.test_createKeyVector
def test_createKeyVector(self)
Definition: test_Utilities.py:43
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
test_Utilities.TestUtilites.test_localToWorld
def test_localToWorld(self)
Definition: test_Utilities.py:206
gtsam::utilities::perturbPose2
void perturbPose2(Values &values, double sigmaT, double sigmaR, int32_t seed=42u)
Perturb all Pose2 values using normally distributed noise.
Definition: nonlinear/utilities.h:217
test_Utilities.TestUtilites.test_allPose2s
def test_allPose2s(self)
Definition: test_Utilities.py:101
test_Utilities.TestUtilites.test_allPose3s
def test_allPose3s(self)
Definition: test_Utilities.py:121
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::utilities::insertBackprojections
void insertBackprojections(Values &values, const PinholeCamera< Cal3_S2 > &camera, const Vector &J, const Matrix &Z, double depth)
Insert a number of initial point values by backprojecting.
Definition: nonlinear/utilities.h:253
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::utilities::perturbPoint2
void perturbPoint2(Values &values, double sigma, int32_t seed=42u)
Perturb all Point2 values using normally distributed noise.
Definition: nonlinear/utilities.h:200
gtsam::utilities::reprojectionErrors
Matrix reprojectionErrors(const NonlinearFactorGraph &graph, const Values &values)
Calculate the errors of all projection factors in a graph.
Definition: nonlinear/utilities.h:294
gtsam::utilities::extractPose3
Matrix extractPose3(const Values &values)
Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z].
Definition: nonlinear/utilities.h:158
gtsam::utilities::createKeyList
FastList< Key > createKeyList(std::string s, const Vector &I)
Definition: nonlinear/utilities.h:49
gtsam::utilities::createKeyVector
KeyVector createKeyVector(std::string s, const Vector &I)
Definition: nonlinear/utilities.h:66
test_Utilities.TestUtilites.test_createKeySet
def test_createKeySet(self)
Definition: test_Utilities.py:63
gtsam::Values
Definition: Values.h:65
test_Utilities.TestUtilites.test_reprojectionErrors
def test_reprojectionErrors(self)
Definition: test_Utilities.py:191
gtsam::utilities::extractPoint2
Matrix extractPoint2(const Values &values)
Extract all Point2 values into a single matrix [x y].
Definition: nonlinear/utilities.h:92
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::utilities::insertProjectionFactors
void insertProjectionFactors(NonlinearFactorGraph &graph, Key i, const Vector &J, const Matrix &Z, const SharedNoiseModel &model, const Cal3_S2::shared_ptr K, const Pose3 &body_P_sensor=Pose3())
Insert multiple projection factors for a single pose key.
Definition: nonlinear/utilities.h:278
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2446
test_Utilities.TestUtilites.test_KeyList_iteration
def test_KeyList_iteration(self)
Definition: test_Utilities.py:32
gtsam::utilities::createKeySet
KeySet createKeySet(std::string s, const Vector &I)
Definition: nonlinear/utilities.h:83
gtsam::Pose2
Definition: Pose2.h:39
test_Utilities.TestUtilites
Definition: test_Utilities.py:20
test_Utilities.TestUtilites.test_extractPose3
def test_extractPose3(self)
Definition: test_Utilities.py:131
gtsam::utilities::perturbPoint3
void perturbPoint3(Values &values, double sigma, int32_t seed=42u)
Perturb all Point3 values using normally distributed noise.
Definition: nonlinear/utilities.h:228


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