Source Code for Module geometry.manifolds.special_euclidean_group

 1  from . import DifferentiableManifold, MatrixLieGroup, SO, se, R, contract 
 2  from .. import (SE3_from_SE2, assert_allclose, pose_from_rotation_translation, 
 3      rotation_translation_from_pose, extract_pieces, se2_from_SE2, SE2_from_se2, 
 4      SE2_from_translation_angle) 
 5   
 6   
 7 -class SE_group(MatrixLieGroup): 
 8      '''  
 9          This is the Special Euclidean group SE(n)  
10          describing roto-translations of Euclidean space. 
11          Implemented only for n=2,3. 
12           
13          Note that you have to supply a coefficient *alpha* that 
14          weights rotation and translation when defining distances.  
15      ''' 
16   
17      @contract(N='int,(2|3)') 
18 -    def __init__(self, N): 
19          algebra = se[N] 
20          self.SOn = SO[N] 
21          self.En = R[N] 
22          dimension = {2: 3, 3: 6}[N] 
23          MatrixLieGroup.__init__(self, n=N + 1, 
24                                  algebra=algebra, dimension=dimension) 
25   
26          DifferentiableManifold.embedding(self, 
27                                           algebra, 
28                                           self.algebra_from_group, 
29                                           self.group_from_algebra, 
30                                           itype='lie') 
31   
32 -    def __repr__(self): 
33          return 'SE%s' % (self.n - 1) 
34   
35      @contract(x='array[NxN]') 
36 -    def belongs(self, x): 
37          # TODO: more checks 
38          assert x.shape == (self.n, self.n) 
39          R, t, zero, one = extract_pieces(x) #@UnusedVariable 
40          self.SOn.belongs(R) 
41          assert_allclose(zero, 0, err_msg='I expect the lower row to be 0.') 
42          assert_allclose(one, 1) 
43   
44 -    def sample_uniform(self): 
45          t = self.En.sample_uniform() 
46          R = self.SOn.sample_uniform() 
47          assert t.size == R.shape[0] 
48          return pose_from_rotation_translation(R, t) 
49   
50 -    def friendly(self, x): 
51          R, t = rotation_translation_from_pose(x) 
52          return 'Pose(%s,%s)' % (self.SOn.friendly(R), self.En.friendly(t)) 
53   
54      # TODO: make explicit inverse 
55      # TODO: make specialization for SE(3) 
56 -    def group_from_algebra(self, g): 
57          if self.n == 3: 
58              return SE2_from_se2(g) 
59          else: 
60              return MatrixLieGroup.group_from_algebra(self, g) 
61   
62 -    def algebra_from_group(self, a): 
63          if self.n == 3: 
64              return se2_from_SE2(a) 
65          else: 
66              return MatrixLieGroup.algebra_from_group(self, a) 
67   
68 -    def interesting_points(self): 
69          if self.n == 3: 
70              return [ 
71                  SE2_from_translation_angle([0, 0], 0), 
72                  SE2_from_translation_angle([0, 0], 0.1), 
73                  SE2_from_translation_angle([0, 0], -0.1), 
74                  SE2_from_translation_angle([1, 0.1], 0), 
75              ] 
76          elif self.n == 4: 
77              # TODO: better implementation 
78              return [ 
79                  SE3_from_SE2(SE2_from_translation_angle([0, 0], 0)), 
80                  SE3_from_SE2(SE2_from_translation_angle([0, 0], 0.1)), 
81                  SE3_from_SE2(SE2_from_translation_angle([0, 0], -0.1)), 
82                  SE3_from_SE2(SE2_from_translation_angle([1, 0.1], 0)), 
83              ] 
84          else: 
85              assert False 
86