Source Code for Module geometry.manifolds.translation_group

 1  from . import MatrixLieGroup, np, R, tran, DifferentiableManifold, contract 
 2  from .. import (assert_allclose, pose_from_rotation_translation, 
 3      rotation_translation_from_pose, extract_pieces) 
 4   
 5   
 6 -class Tran(MatrixLieGroup): 
 7      '''  
 8          The translation subgroup of SE(n).  
 9      ''' 
10   
11      @contract(n='1|2|3') 
12 -    def __init__(self, n): 
13          algebra = tran[n] 
14          MatrixLieGroup.__init__(self, n=n + 1, algebra=algebra, dimension=n) 
15          self.En = R[n] 
16          DifferentiableManifold.isomorphism(self, algebra, 
17                              self.algebra_from_group, 
18                              self.group_from_algebra, 
19                              itype='lie') 
20   
21 -    def __repr__(self): 
22          #return 'Tran(%s)' % (self.n - 1) 
23          return 'Tr%s' % (self.n - 1) 
24   
25 -    def belongs(self, x): 
26          # TODO: explicit 
27          R, t, zero, one = extract_pieces(x) #@UnusedVariable 
28          assert_allclose(R, np.eye(self.n - 1)) 
29          assert_allclose(zero, 0, err_msg='I expect the lower row to be 0.') 
30          assert_allclose(one, 1, err_msg='Bottom-right must be 1.') 
31   
32      @contract(returns='belongs') 
33 -    def sample_uniform(self): 
34          t = self.En.sample_uniform() 
35          return pose_from_rotation_translation(np.eye(self.n - 1), t) 
36   
37      @contract(x='belongs') 
38 -    def friendly(self, x): 
39          t = rotation_translation_from_pose(x)[1] 
40          return 'Tran(%s)' % (self.En.friendly(t)) 
41   
42      @contract(base='belongs', target='belongs', returns='belongs_ts') 
43 -    def logmap(self, base, target): 
44          return base, target - base 
45   
46      @contract(bv='belongs_ts', returns='belongs') 
47 -    def expmap(self, bv): 
48          base, vel = bv 
49          return base + vel 
50   
51      @contract(g='belongs', returns='belongs_algebra') 
52 -    def algebra_from_group(self, g): 
53          a = np.zeros((self.n, self.n)) 
54          a[:-1, -1] = g[:-1, -1] 
55          return a 
56   
57      @contract(a='belongs_algebra', returns='belongs') 
58 -    def group_from_algebra(self, a): 
59          g = np.eye(self.n) 
60          g[:-1, -1] = a[:-1, -1] 
61          return g 
62   
63      @contract(returns='list(belongs)') 
64 -    def interesting_points(self): 
65          points = [] 
66          for t in self.En.interesting_points(): 
67              p = pose_from_rotation_translation(np.eye(self.n - 1), t) 
68              points.append(p) 
69   
70          return points 
71