-
Notifications
You must be signed in to change notification settings - Fork 3
/
transform.py
203 lines (160 loc) · 5.86 KB
/
transform.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
#
# This file is part of wganvo.
# This file is based on a file from https://github.com/ori-mrg/robotcar-dataset-sdk
# (see original license below)
#
# Modifications copyright (C) 2019 Javier Cremona (CIFASIS-CONICET)
# For more information see <https://github.com/CIFASIS/wganvo>
#
# This file is licensed under the Creative Commons
# Attribution-NonCommercial-ShareAlike 4.0 International License.
# To view a copy of this license, visit
# http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to
# Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
#
################################################################################
#
# Copyright (c) 2017 University of Oxford
# Authors:
# Geoff Pascoe (gmp@robots.ox.ac.uk)
#
# This work is licensed under the Creative Commons
# Attribution-NonCommercial-ShareAlike 4.0 International License.
# To view a copy of this license, visit
# http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to
# Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
#
################################################################################
import numpy as np
import numpy.matlib as matlib
from math import sin, cos, atan2, sqrt
MATRIX_MATCH_TOLERANCE = 1e-4
def build_intrinsic_matrix(focal_length, principal_point):
intrinsic_matrix = matlib.identity(3)
intrinsic_matrix[0:2, 2] = np.matrix(principal_point).transpose()
intrinsic_matrix[0,0] = focal_length[0]
intrinsic_matrix[1,1] = focal_length[1]
return intrinsic_matrix
def build_se3_transform(xyzrpy):
"""Creates an SE3 transform from translation and Euler angles.
Args:
xyzrpy (list[float]): translation and Euler angles for transform. Must have six components.
Returns:
numpy.matrixlib.defmatrix.matrix: SE3 homogeneous transformation matrix
Raises:
ValueError: if `len(xyzrpy) != 6`
"""
if len(xyzrpy) != 6:
raise ValueError("Must supply 6 values to build transform")
se3 = matlib.identity(4)
se3[0:3, 0:3] = euler_to_so3(xyzrpy[3:6])
se3[0:3, 3] = np.matrix(xyzrpy[0:3]).transpose()
return se3
def euler_to_so3(rpy):
"""Converts Euler angles to an SO3 rotation matrix.
Args:
rpy (list[float]): Euler angles (in radians). Must have three components.
Returns:
numpy.matrixlib.defmatrix.matrix: 3x3 SO3 rotation matrix
Raises:
ValueError: if `len(rpy) != 3`.
"""
if len(rpy) != 3:
raise ValueError("Euler angles must have three components")
R_x = np.matrix([[1, 0, 0],
[0, cos(rpy[0]), -sin(rpy[0])],
[0, sin(rpy[0]), cos(rpy[0])]])
R_y = np.matrix([[cos(rpy[1]), 0, sin(rpy[1])],
[0, 1, 0],
[-sin(rpy[1]), 0, cos(rpy[1])]])
R_z = np.matrix([[cos(rpy[2]), -sin(rpy[2]), 0],
[sin(rpy[2]), cos(rpy[2]), 0],
[0, 0, 1]])
R_zyx = R_z * R_y * R_x
return R_zyx
def so3_to_euler(so3):
"""Converts an SO3 rotation matrix to Euler angles
Args:
so3: 3x3 rotation matrix
Returns:
numpy.matrixlib.defmatrix.matrix: list of Euler angles (size 3)
Raises:
ValueError: if so3 is not 3x3
ValueError: if a valid Euler parametrisation cannot be found
"""
if so3.shape != (3, 3):
raise ValueError("SO3 matrix must be 3x3")
roll = atan2(so3[2, 1], so3[2, 2])
yaw = atan2(so3[1, 0], so3[0, 0])
denom = sqrt(so3[0, 0] ** 2 + so3[1, 0] ** 2)
pitch_poss = [atan2(-so3[2, 0], denom), atan2(-so3[2, 0], -denom)]
R = euler_to_so3((roll, pitch_poss[0], yaw))
if (so3 - R).sum() < MATRIX_MATCH_TOLERANCE:
return np.matrix([roll, pitch_poss[0], yaw])
else:
R = euler_to_so3((roll, pitch_poss[1], yaw))
if (so3 - R).sum() > MATRIX_MATCH_TOLERANCE:
raise ValueError("Could not find valid pitch angle")
return np.matrix([roll, pitch_poss[1], yaw])
def so3_to_quaternion(so3):
"""Converts an SO3 rotation matrix to a quaternion
Args:
so3: 3x3 rotation matrix
Returns:
numpy.ndarray: quaternion [w, x, y, z]
Raises:
ValueError: if so3 is not 3x3
"""
if so3.shape != (3, 3):
raise ValueError("SO3 matrix must be 3x3")
R_xx = so3[0, 0]
R_xy = so3[0, 1]
R_xz = so3[0, 2]
R_yx = so3[1, 0]
R_yy = so3[1, 1]
R_yz = so3[1, 2]
R_zx = so3[2, 0]
R_zy = so3[2, 1]
R_zz = so3[2, 2]
try:
w = sqrt(so3.trace() + 1) / 2
except(ValueError):
# w is non-real
w = 0
x = sqrt(1 + R_xx - R_yy - R_zz) / 2
y = sqrt(1 + R_yy - R_xx - R_zz) / 2
z = sqrt(1 + R_zz - R_yy - R_xx) / 2
max_index = max(range(4), key=[w, x, y, z].__getitem__)
if max_index == 0:
x = (R_zy - R_yz) / (4 * w)
y = (R_xz - R_zx) / (4 * w)
z = (R_yx - R_xy) / (4 * w)
elif max_index == 1:
w = (R_zy - R_yz) / (4 * x)
y = (R_xy + R_yx) / (4 * x)
z = (R_zx + R_xz) / (4 * x)
elif max_index == 2:
w = (R_xz - R_zx) / (4 * y)
x = (R_xy + R_yx) / (4 * y)
z = (R_yz + R_zy) / (4 * y)
elif max_index == 3:
w = (R_yx - R_xy) / (4 * z)
x = (R_zx + R_xz) / (4 * z)
y = (R_yz + R_zy) / (4 * z)
return np.array([w, x, y, z])
def se3_to_components(se3):
"""Converts an SE3 rotation matrix to linear translation and Euler angles
Args:
se3: 4x4 transformation matrix
Returns:
numpy.matrixlib.defmatrix.matrix: list of [x, y, z, roll, pitch, yaw]
Raises:
ValueError: if se3 is not 4x4
ValueError: if a valid Euler parametrisation cannot be found
"""
if se3.shape != (4, 4):
raise ValueError("SE3 transform must be a 4x4 matrix")
xyzrpy = np.empty(6)
xyzrpy[0:3] = se3[0:3, 3].transpose()
xyzrpy[3:6] = so3_to_euler(se3[0:3, 0:3])
return xyzrpy