12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed this test with V4 compatibility (where Points are not initialized)

release/4.3a0
dellaert 2017-08-07 08:37:37 -07:00
parent c5a0f1a839
commit d712498c08
1 changed files with 27 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
cython/gtsam/tests/test_Values.py
View File

@ -1,25 +1,28 @@
import unittest
import gtsam
import numpy as np
from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3
from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias
class TestValues(unittest.TestCase):
def test_values(self):
values = gtsam.Values()
E = gtsam.EssentialMatrix(gtsam.Rot3(), gtsam.Unit3())
values = Values()
E = EssentialMatrix(Rot3(), Unit3())
tol = 1e-9
values.insert(0, gtsam.Point2())
values.insert(1, gtsam.Point3())
values.insert(2, gtsam.Rot2())
values.insert(3, gtsam.Pose2())
values.insert(4, gtsam.Rot3())
values.insert(5, gtsam.Pose3())
values.insert(6, gtsam.Cal3_S2())
values.insert(7, gtsam.Cal3DS2())
values.insert(8, gtsam.Cal3Bundler())
values.insert(0, Point2(0,0))
values.insert(1, Point3(0,0,0))
values.insert(2, Rot2())
values.insert(3, Pose2())
values.insert(4, Rot3())
values.insert(5, Pose3())
values.insert(6, Cal3_S2())
values.insert(7, Cal3DS2())
values.insert(8, Cal3Bundler())
values.insert(9, E)
values.insert(10, gtsam.imuBias_ConstantBias())
values.insert(10, imuBias_ConstantBias())
# Special cases for Vectors and Matrices
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
@ -29,7 +32,7 @@ class TestValues(unittest.TestCase):
# will have 'int' type.
#
# The wrapper will automatically fix the type and storage order for you,
# but for performace reasons, it's recommended to specify the correct
# but for performance reasons, it's recommended to specify the correct
# type and storage order.
vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is
values.insert(11, vec)
@ -41,18 +44,18 @@ class TestValues(unittest.TestCase):
mat2 = np.array([[1,2,],[3,5]])
values.insert(13, mat2)
self.assertTrue(values.atPoint2(0).equals(gtsam.Point2(), tol))
self.assertTrue(values.atPoint3(1).equals(gtsam.Point3(), tol))
self.assertTrue(values.atRot2(2).equals(gtsam.Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(gtsam.Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(gtsam.Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(gtsam.Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(gtsam.Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(gtsam.Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(gtsam.Cal3Bundler(), tol))
self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
self.assertTrue(values.atimuBias_ConstantBias(
10).equals(gtsam.imuBias_ConstantBias(), tol))
10).equals(imuBias_ConstantBias(), tol))
# special cases for Vector and Matrix:
actualVector = values.atVector(11)