Source Code for Module geometry.manifolds.special_euclidean_algebra

 1  from . import np, MatrixLieAlgebra, so, contract 
 2  from .. import extract_pieces, combine_pieces, hat_map_2d, hat_map 
 3   
 4   
 5 -class se_algebra(MatrixLieAlgebra): 
 6      ''' This is the Lie algebra se(n) for the Special Euclidean group SE(n).  
 7       
 8          Note that you have to supply a coefficient *alpha* that 
 9          weights rotation and translation when defining distances.  
10      ''' 
11   
12 -    def __init__(self, N, alpha): 
13          dimension = {2: 3, 3: 6}[N] 
14          MatrixLieAlgebra.__init__(self, n=N + 1, dimension=dimension) 
15          self.alpha = alpha 
16          self.son = so[N] 
17   
18 -    def norm(self, X): 
19          W, v, zero, zero = extract_pieces(X) #@UnusedVariable 
20          return np.linalg.norm(v) + self.alpha * self.son.norm(W) 
21   
22 -    def project(self, X): 
23          W, v, zero, zero = extract_pieces(X) #@UnusedVariable 
24          W = self.son.project(W) 
25          return combine_pieces(W, v, v * 0, 0) 
26   
27 -    def __repr__(self): 
28          #return 'se(%s)' % (self.n - 1) 
29          return 'se%s' % (self.n - 1) 
30   
31      @contract(a='belongs') 
32 -    def vector_from_algebra(self, a): 
33          W, v, zero, zero = extract_pieces(a) #@UnusedVariable 
34   
35          if self.n == 3: 
36              assert v.size == 2 
37              V = np.zeros(3) 
38              V[0] = self.son.vector_from_algebra(W) 
39              V[1:3] = v 
40              return V 
41          elif self.n == 4: 
42              assert v.size == 3 
43              V = np.zeros(6) 
44              V[0:3] = self.son.vector_from_algebra(W) 
45              V[3:6] = v 
46              return V 
47          else: 
48              assert False, 'Not implemented for n>=4.' 
49   
50      @contract(v='array[N]', returns='belongs') 
51 -    def algebra_from_vector(self, v): 
52          if self.n == 3: 
53              assert v.size == 3 
54              omega = v[0] 
55              vel = v[1:3] 
56              W = hat_map_2d(omega) 
57              return combine_pieces(W, vel, vel * 0, 0) 
58   
59          elif self.n == 4: 
60              assert v.size == 6 
61              omega = v[0:3] 
62              vel = v[3:6] 
63              W = hat_map(omega) 
64              return combine_pieces(W, vel, vel * 0, 0) 
65          else: 
66              assert False, 'Not implemented for n=%d.' % self.n 
67   
68 -    def interesting_points(self): 
69          points = [] 
70          points.append(self.zero()) 
71          if self.n == 3: 
72              from . import SE2 
73              points.extend([SE2.algebra_from_group(p) 
74                             for p in SE2.interesting_points()]) 
75          elif self.n == 4: 
76              from . import SE3 
77              points.extend([SE3.algebra_from_group(p) 
78                             for p in SE3.interesting_points()]) 
79          else: 
80              assert False, 'Not implemented for n=%s' % self.n 
81          return points 
82