test_Values.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 Values unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 # pylint: disable=invalid-name, E1101, E0611
12 import unittest
13 
14 import numpy as np
15 
16 import gtsam
17 from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
18  Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias)
19 from gtsam.utils.test_case import GtsamTestCase
20 
21 
23 
24  def test_values(self):
25  values = Values()
26  E = EssentialMatrix(Rot3(), Unit3())
27  tol = 1e-9
28 
29  values.insert(0, Point2(0, 0))
30  values.insert(1, Point3(0, 0, 0))
31  values.insert(2, Rot2())
32  values.insert(3, Pose2())
33  values.insert(4, Rot3())
34  values.insert(5, Pose3())
35  values.insert(6, Cal3_S2())
36  values.insert(7, Cal3DS2())
37  values.insert(8, Cal3Bundler())
38  values.insert(9, E)
39  values.insert(10, imuBias.ConstantBias())
40 
41  # Special cases for Vectors and Matrices
42  # Note that gtsam's Eigen Vectors and Matrices requires double-precision
43  # floating point numbers in column-major (Fortran style) storage order,
44  # whereas by default, numpy.array is in row-major order and the type is
45  # in whatever the number input type is, e.g. np.array([1,2,3])
46  # will have 'int' type.
47  #
48  # The wrapper will automatically fix the type and storage order for you,
49  # but for performance reasons, it's recommended to specify the correct
50  # type and storage order.
51  # for vectors, the order is not important, but dtype still is
52  vec = np.array([1., 2., 3.])
53  values.insert(11, vec)
54  mat = np.array([[1., 2.], [3., 4.]], order='F')
55  values.insert(12, mat)
56  # Test with dtype int and the default order='C'
57  # This still works as the wrapper converts to the correct type and order for you
58  # but is nornally not recommended!
59  mat2 = np.array([[1, 2, ], [3, 5]])
60  values.insert(13, mat2)
61 
62  self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol)
63  self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol)
64  self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol)
65  self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol)
66  self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol)
67  self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol)
68  self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol)
69  self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
70  self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
71  self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
72  self.gtsamAssertEquals(values.atConstantBias(
73  10), imuBias.ConstantBias(), tol)
74 
75  # special cases for Vector and Matrix:
76  actualVector = values.atVector(11)
77  np.testing.assert_allclose(vec, actualVector, tol)
78  actualMatrix = values.atMatrix(12)
79  np.testing.assert_allclose(mat, actualMatrix, tol)
80  actualMatrix2 = values.atMatrix(13)
81  np.testing.assert_allclose(mat2, actualMatrix2, tol)
82 
83 
84 if __name__ == "__main__":
85  unittest.main()
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
Vector2 Point2
Definition: Point2.h:32
Vector3 Point3
Definition: Point3.h:38


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