Source Code for Module geometry.poses_embedding

 1  from . import (SE3_from_rotation_translation, combine_pieces, extract_pieces, 
 2      contract, np, SE2_from_rotation_translation, rotation_translation_from_SE2, 
 3      rotation_translation_from_SE3, hat_map, map_hat_2d, SO2_project_from_SO3, 
 4      so2_project_from_so3, so3_from_so2) 
 5   
 6   
 7  @contract(returns='SE2', a='SO2') 
 8 -def SE2_from_SO2(a): 
 9      return SE2_from_rotation_translation(a, np.array([0, 0])) 
10   
11   
12  @contract(returns='SO2', b='SE2') 
13 -def SO2_project_from_SE2(b): 
14      return rotation_translation_from_SE2(b)[0] 
15   
16   
17  @contract(returns='se2', a='so2') 
18 -def se2_from_so2(a): 
19      omega = map_hat_2d(a) 
20      return hat_map(np.array([0, 0, omega])) 
21   
22   
23  @contract(returns='so2', b='se2') 
24 -def so2_project_from_se2(b): 
25      return extract_pieces(b)[0] 
26   
27   
28  @contract(returns='SE3', a='SO3') 
29 -def SE3_from_SO3(a): 
30      return SE3_from_rotation_translation(a, np.array([0, 0, 0])) 
31   
32   
33  @contract(returns='SO3', b='SE3') 
34 -def SO3_project_from_SE3(b): 
35      return rotation_translation_from_SE3(b)[0] 
36   
37   
38  @contract(returns='se3', a='so3') 
39 -def se3_from_so3(a): 
40      return combine_pieces(a, np.array([0, 0, 0]), np.array([0, 0, 0]), 0) 
41   
42   
43  @contract(returns='so3', b='se3') 
44 -def so3_project_from_se3(b): 
45      return extract_pieces(b)[0] 
46   
47   
48  @contract(returns='SE2', a='R2') 
49 -def SE2_from_R2(a): 
50      return SE2_from_rotation_translation(np.eye(2), a) 
51   
52   
53  @contract(returns='SE3', a='R3') 
54 -def SE3_from_R3(a): 
55      return SE3_from_rotation_translation(np.eye(3), a) 
56   
57   
58  @contract(returns='R2', b='SE2') 
59 -def R2_project_from_SE2(b): 
60      return rotation_translation_from_SE2(b)[1] 
61   
62   
63  @contract(returns='R3', b='SE3') 
64 -def R3_project_from_SE3(b): 
65      return rotation_translation_from_SE3(b)[1] 
66   
67   
68  @contract(returns='se3', a='se2') 
69 -def se3_from_se2(a): 
70      W, v, zero, one = extract_pieces(a) #@UnusedVariable 
71      W = so3_from_so2(W) 
72      v = np.array([v[0], v[1], 0]) 
73      return combine_pieces(W, v, v * 0, 0) 
74   
75   
76  @contract(returns='SE2', b='SE3') 
77 -def SE2_project_from_SE3(b): 
78      R, t, zero, one = extract_pieces(b) #@UnusedVariable 
79      R = SO2_project_from_SO3(R) 
80      t = t[0:2] 
81      return combine_pieces(R, t, t * 0, 1) 
82   
83   
84  @contract(returns='se2', b='se3') 
85 -def se2_project_from_se3(b): 
86      W, v, zero, one = extract_pieces(b) #@UnusedVariable 
87      W = so2_project_from_so3(W) 
88      v = v[0:2] 
89      return combine_pieces(W, v, v * 0, 0) 
90