← Back to branch summary

~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to test/pose3d_test.py

  • Committer: Julian Ryde
  • Date: 2011-06-03 16:52:15 UTC
  • Revision ID: julian_ryde-20110603165215-oqr6yo9nwng3j9nb
Initial import from revision 206 of Julian Ryde's mrol_mapping repository.

Show diffs side-by-side

added added

removed removed

Lines of Context:
test/pose3d_test.py
 
1
#!/usr/bin/env python
 
2
import numpy as np
 
3
import mrol_mapping.poseutil as poseutil
 
4
import unittest
 
5
 
 
6
class Testposeutil(unittest.TestCase):
 
7
    def setUp(self):
 
8
        self.p = []
 
9
        self.p.append(poseutil.Pose3D())
 
10
        self.p.append(poseutil.Pose3D())
 
11
 
 
12
    def testtuple_matrix(self):
 
13
        X = (0, 0, 0, 0, 0, 0)
 
14
        Y =  poseutil.posetuple(poseutil.mat(X))
 
15
        self.assertEquals(X, Y)
 
16
        X = (0.1, -0.2, 0, 0.45, 0.9, -1.1)
 
17
        Y =  poseutil.posetuple(poseutil.mat(X))
 
18
        self.assertTrue(np.allclose(X, Y))
 
19
 
 
20
    def testeqandhash(self):
 
21
        P = self.p
 
22
        P = poseutil.Pose3D()
 
23
        P.set([1.2, 2.3, 0.2, 0.12, 0.63, 0.9])
 
24
        Q = poseutil.Pose3D()
 
25
        Q.setMatrix(P.getMatrix())
 
26
 
 
27
        self.assertEquals(P, Q)
 
28
 
 
29
    def testsetget(self):
 
30
        p = self.p
 
31
        S = set()
 
32
        S.add(p[0])
 
33
        S.add(p[1])
 
34
        self.assertEquals(len(S), 1)
 
35
        S = set()
 
36
        p[1].x = 1
 
37
        S.add(p[0])
 
38
        S.add(p[1])
 
39
        self.assertEquals(len(S), 2)
 
40
        #self.assertEquals(self.p[0][0], 0)
 
41
        #self.assertEquals(self.p[1][5], 1)
 
42
 
 
43
    def testcombined(self):
 
44
        p = self.p[0]
 
45
        combo = p.getCombined(p)
 
46
        self.assertTrue(np.allclose(combo.get(), p.get()))
 
47
        p.set([1, 1, 0, 0, 0, np.radians(45)])
 
48
        combo = p.getCombined(p)
 
49
        expected = poseutil.Pose3D()
 
50
        expected.set([1, 1+2**0.5, 0, 0, 0, np.radians(90)])
 
51
        self.assertTrue(np.allclose(expected.get(), combo.get()))
 
52
        # TODO 3D tests still required
 
53
 
 
54
    def testtransform(self):
 
55
        p = self.p
 
56
        p[0].rotz = np.radians(90)
 
57
        X = np.matrix('0, 0, 0; 1, 0, 0')
 
58
        Y = p[0].transformPoints(X)
 
59
        expected = np.matrix('0, 0, 0; 0, 1, 0')
 
60
        self.assertTrue(np.allclose(Y, expected)) # pure rotation
 
61
        p[1].x = 1
 
62
        p[1].y = 2
 
63
        p[1].z = 3
 
64
        Y = p[1].transformPoints(X)
 
65
        expected = np.matrix('1, 2, 3; 2, 2, 3')
 
66
        self.assertTrue(np.allclose(Y, expected)) # pure translation
 
67
        p[0].x = 1
 
68
        Y = p[0].transformPoints(X)
 
69
        expected = np.matrix('1, 0, 0; 1, 1, 0')
 
70
        # it rotates first then displaces
 
71
        self.assertTrue(np.allclose(Y, expected)) # combined
 
72
 
 
73
    def testinverse(self):
 
74
        p = self.p
 
75
        q = p[0].inverse().inverse()
 
76
        self.assertEquals(p[0], q)
 
77
 
 
78
    def testcopy(self):
 
79
        p = self.p
 
80
        p[0].x = 1
 
81
        p[0].rotx = 2
 
82
        c = p[0].copy()
 
83
        self.assertFalse(c is p[0])
 
84
        self.assertEqual(c, p[0])
 
85
 
 
86
if __name__ == '__main__':
 
87
    unittest.main()
 
88