This repository has been archived by the owner on Feb 3, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 5
/
rotation.py
98 lines (78 loc) · 2.92 KB
/
rotation.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
import numpy as np
def estimate_normal(planar_points):
from sklearn.decomposition import PCA
pca = PCA(n_components=3)
normal = pca.fit(planar_points).components_[-1]
normal /= np.linalg.norm(normal)
return normal
def rotate_to_xz_plane(points, normal=None):
'''
Rotates points to the x-z plane. If the initial center
of mass is not to within 1e-5 of the origin, we
translate it to the origin.
Returns (r, R, p0):
- r is the rotated image of points,
- R is the rotation matrix
- p0 is the translation factor (can be None)
'''
import cv2
from blmath.geometry.transform.translation import translation
if points is None or not len(points): # pylint: disable=len-as-condition
raise ValueError('Some points are required')
center = np.mean(points, axis=0)
if np.linalg.norm(center) > 1e-5:
translated, p0 = translation(points)
else:
translated, p0 = points, None
if not normal:
normal = estimate_normal(points)
e_2 = np.array([0., 1., 0.])
theta = np.arccos(np.dot(e_2, normal))
if min(abs(theta - np.pi), abs(theta)) < 1e-5:
# cross product will degenerate
# to zero vector in this case
r_axis = np.array([1., 0., 0.])
else:
r_axis = np.cross(normal, e_2)
r_axis /= np.linalg.norm(r_axis)
R = cv2.Rodrigues(theta * r_axis)[0]
rotated = np.dot(translated, R.T)
return (rotated, R, p0)
def rotation_from_up_and_look(up, look):
'''
Rotation matrix to rotate a mesh into a canonical reference frame. The
result is a rotation matrix that will make up along +y and look along +z
(i.e. facing towards a default opengl camera).
Note that if you're reorienting a mesh, you can use its `reorient` method
to accomplish this.
up: The foot-to-head direction.
look: The direction the eyes are facing, or the heel-to-toe direction.
'''
up, look = np.array(up, dtype=np.float64), np.array(look, dtype=np.float64)
if np.linalg.norm(up) == 0:
raise ValueError("Singular up")
if np.linalg.norm(look) == 0:
raise ValueError("Singular look")
y = up / np.linalg.norm(up)
z = look - np.dot(look, y)*y
if np.linalg.norm(z) == 0:
raise ValueError("up and look are colinear")
z = z / np.linalg.norm(z)
x = np.cross(y, z)
return np.array([x, y, z])
def euler(xyz, order='xyz', units='deg'):
if not hasattr(xyz, '__iter__'):
xyz = [xyz]
if units == 'deg':
xyz = np.radians(xyz)
r = np.eye(3)
for theta, axis in zip(xyz, order):
c = np.cos(theta)
s = np.sin(theta)
if axis == 'x':
r = np.dot(np.array([[1, 0, 0], [0, c, -s], [0, s, c]]), r)
if axis == 'y':
r = np.dot(np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]), r)
if axis == 'z':
r = np.dot(np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]), r)
return r