├── HPE_heatmap_generation.py
├── README.md
├── SVD_non_singular_equation.py
├── check_ip.py
├── human36m_3d_visualization.py
├── least_square.py
├── per_image_standardization_each_channel.py
├── resource
├── gt.npy
├── h36m_3d_visualization.gif
└── lena.jpg
├── soft_argmax.py
├── test.py
└── transformations.py
/HPE_heatmap_generation.py:
--------------------------------------------------------------------------------
1 | import time
2 | import numpy as np
3 | import cv2
4 | import matplotlib.pyplot as plt
5 |
6 |
7 | def CenterLabelHeatMap(img_width, img_height, c_x, c_y, sigma):
8 | X1 = np.linspace(1, img_width, img_width)
9 | Y1 = np.linspace(1, img_height, img_height)
10 | [X, Y] = np.meshgrid(X1, Y1)
11 | X = X - c_x
12 | Y = Y - c_y
13 | D2 = X * X + Y * Y
14 | E2 = 2.0 * sigma * sigma
15 | Exponent = D2 / E2
16 | heatmap = np.exp(-Exponent)
17 | return heatmap
18 |
19 |
20 | # Compute gaussian kernel
21 | def CenterGaussianHeatMap(img_height, img_width, c_x, c_y, variance):
22 | gaussian_map = np.zeros((img_height, img_width))
23 | for x_p in range(img_width):
24 | for y_p in range(img_height):
25 | dist_sq = (x_p - c_x) * (x_p - c_x) + \
26 | (y_p - c_y) * (y_p - c_y)
27 | exponent = dist_sq / 2.0 / variance / variance
28 | gaussian_map[y_p, x_p] = np.exp(-exponent)
29 | return gaussian_map
30 |
31 |
32 | if __name__ == "__main__":
33 | image_file = 'resource/lena.jpg'
34 | img = cv2.imread(image_file)
35 | img = img[:, :, ::-1]
36 |
37 | height, width, _ = np.shape(img)
38 | cy, cx = height / 2.0, width / 2.0
39 |
40 | start = time.time()
41 | heatmap1 = CenterLabelHeatMap(width, height, cx, cy, 21)
42 | t1 = time.time() - start
43 |
44 | start = time.time()
45 | heatmap2 = CenterGaussianHeatMap(height, width, cx, cy, 21)
46 | t2 = time.time() - start
47 |
48 | print(t1, t2)
49 |
50 | plt.subplot(1, 2, 1)
51 | plt.imshow(heatmap1)
52 | plt.subplot(1, 2, 2)
53 | plt.imshow(heatmap2)
54 | plt.show()
55 |
56 | print('End.')
57 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # PaperDemoAndExampleTest
2 | Testing code that verifies the correctness of the formula or practice in the papers I'm reading
3 |
4 | #### human36m 3d pose visualization ([blog link](https://blog.csdn.net/HelloWorld1108/article/details/103265652#comments_15868989))
5 |
6 | 
7 |
--------------------------------------------------------------------------------
/SVD_non_singular_equation.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 |
3 | a = np.array([[1.1, -0.1],
4 | [-0.1, 1.1]])
5 |
6 | b = np.array([[86.6], [103.4]])
7 |
8 | u, d, v = np.linalg.svd(a)
9 |
10 | # print(u, d, v)
11 |
12 | b_ = u.T @ b
13 |
14 | y = [b_[i, 0] / val for i, val in enumerate(d)]
15 |
16 | x = v.T @ np.expand_dims(np.array(y), 1)
17 |
18 | print(x)
19 |
--------------------------------------------------------------------------------
/check_ip.py:
--------------------------------------------------------------------------------
1 | import os
2 |
3 | ava_ips = []
4 | allocated_ips = []
5 | for i in range(2, 255):
6 | command = os.system('ping -c 1 10.21.243.' + str(i))
7 | if command == 0:
8 | allocated_ips.append(i)
9 | pass # Sucess
10 | else:
11 | ava_ips.append(i)
12 | # print(str(i))
13 |
14 | print('allocated_ips', allocated_ips)
15 | print('ava_ips', ava_ips)
16 |
--------------------------------------------------------------------------------
/human36m_3d_visualization.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | import numpy as np
3 | import transformations
4 |
5 |
6 | class Camera:
7 | def __init__(self, R, t, K, dist=None, name=""):
8 | self.R = R.copy()
9 | self.t = t.copy()
10 | self.K = K.copy()
11 | self.dist = dist
12 |
13 | self.name = name
14 |
15 | def update_after_crop(self, bbox):
16 | left, upper, right, lower = bbox
17 |
18 | cx, cy = self.K[0, 2], self.K[1, 2]
19 |
20 | new_cx = cx - left
21 | new_cy = cy - upper
22 |
23 | self.K[0, 2], self.K[1, 2] = new_cx, new_cy
24 |
25 | def update_after_resize(self, image_shape, new_image_shape):
26 | height, width = image_shape
27 | new_width, new_height = new_image_shape
28 |
29 | fx, fy, cx, cy = self.K[0, 0], self.K[1, 1], self.K[0, 2], self.K[1, 2]
30 |
31 | new_fx = fx * (new_width / width)
32 | new_fy = fy * (new_height / height)
33 | new_cx = cx * (new_width / width)
34 | new_cy = cy * (new_height / height)
35 |
36 | self.K[0, 0], self.K[1, 1], self.K[0, 2], self.K[1, 2] = new_fx, new_fy, new_cx, new_cy
37 |
38 | @property
39 | def projection(self):
40 | return self.K.dot(self.extrinsics)
41 |
42 | @property
43 | def extrinsics(self):
44 | return np.hstack([self.R, self.t])
45 |
46 |
47 | def generate_grid_mesh(start, end, step=1.0):
48 | num_point_per_line = int((end - start) // step + 1)
49 | its = np.linspace(start, end, num_point_per_line)
50 | line = []
51 | color = []
52 | common_line_color = [192, 192, 192]
53 | for i in range(num_point_per_line):
54 | line.append([its[0], its[i], 0, its[-1], its[i], 0])
55 | if its[i] == 0:
56 | color.append([0, 255, 0])
57 | else:
58 | color.append(common_line_color)
59 |
60 | for i in range(num_point_per_line):
61 | line.append([its[i], its[-1], 0, its[i], its[0], 0])
62 | if its[i] == 0:
63 | color.append([0, 0, 255])
64 | else:
65 | color.append(common_line_color)
66 |
67 | return np.array(line, dtype=np.float32), np.array(color, dtype=np.uint8)
68 |
69 |
70 | def euclidean_to_homogeneous(points):
71 | if isinstance(points, np.ndarray):
72 | return np.hstack([points, np.ones((len(points), 1))])
73 | else:
74 | raise TypeError("Works only with numpy arrays")
75 |
76 |
77 | def homogeneous_to_euclidean(points):
78 | if isinstance(points, np.ndarray):
79 | return (points.T[:-1] / points.T[-1]).T
80 | else:
81 | raise TypeError("Works only with numpy arrays")
82 |
83 |
84 | def projection_to_2d_plane(vertices, projection_matrix, view_matrix=None, scale=None):
85 | if view_matrix is not None:
86 | vertices = (homogeneous_to_euclidean(
87 | (euclidean_to_homogeneous(vertices) @ view_matrix.T) @ projection_matrix.T)[:, :2]) * scale
88 |
89 | vertices[:, 1] = scale - vertices[:, 1]
90 | vertices[:, 0] = vertices[:, 0] + scale
91 | else:
92 | vertices = euclidean_to_homogeneous(vertices) @ projection_matrix.T
93 | vertices = homogeneous_to_euclidean(vertices)
94 | return vertices.astype(np.int32)
95 |
96 |
97 | def look_at(eye, center, up):
98 | f = unit_vector(center - eye)
99 | u = unit_vector(up)
100 | s = unit_vector(np.cross(f, u))
101 | u = np.cross(s, f)
102 |
103 | result = transformations.identity_matrix()
104 | result[:3, 0] = s
105 | result[:3, 1] = u
106 | result[:3, 2] = -f
107 | result[3, 0] = -np.dot(s, eye)
108 | result[3, 1] = -np.dot(u, eye)
109 | result[3, 2] = np.dot(f, eye)
110 | return result.T
111 |
112 |
113 | def unit_vector(vector):
114 | return vector / np.linalg.norm(vector)
115 |
116 |
117 | def update_camera_vectors():
118 | global front
119 | front_temp = np.zeros((3,))
120 | front_temp[0] = np.cos(np.radians(yaw)) * np.cos(np.radians(pitch))
121 | front_temp[1] = np.sin(np.radians(pitch))
122 | front_temp[2] = np.sin(np.radians(yaw)) * np.cos(np.radians(pitch))
123 | front = unit_vector(front_temp)
124 | global right
125 | right = unit_vector(np.cross(front, world_up))
126 |
127 |
128 | camera_vertices = np.array([[0, 0, 0], [-1, -1, 2],
129 | [0, 0, 0], [1, 1, 2],
130 | [0, 0, 0], [1, -1, 2],
131 | [0, 0, 0], [-1, 1, 2],
132 | [-1, 1, 2], [-1, -1, 2],
133 | [-1, -1, 2], [1, -1, 2],
134 | [1, -1, 2], [1, 1, 2],
135 | [1, 1, 2], [-1, 1, 2]], dtype=np.float32)
136 |
137 | human36m_connectivity_dict = [(0, 1), (1, 2), (2, 6), (5, 4), (4, 3), (3, 6), (6, 7), (7, 8), (8, 16), (9, 16), (8, 12),
138 | (11, 12), (10, 11), (8, 13), (13, 14), (14, 15)]
139 |
140 | multiview_data = np.load("/home/shuai/Downloads/3D_sence_multiview.npy", allow_pickle=True).tolist()
141 | subject_name, camera_name, action_name, camera_configs, labels = multiview_data['subject_names'], multiview_data[
142 | 'camera_names'], multiview_data['action_names'], multiview_data['cameras'], multiview_data['table']
143 |
144 | camera_name = [str(i) for i, c in enumerate(camera_name)]
145 |
146 | # subject_name ['S1', 'S5', 'S6', 'S7', 'S8', 'S9', 'S11']
147 | # action_name ['Directions-1', 'Directions-2', 'Discussion-1', 'Discussion-2', 'Eating-1', 'Eating-2', 'Greeting-1', 'Greeting-2', 'Phoning-1', 'Phoning-2', 'Posing-1', 'Posing-2', 'Purchases-1', 'Purchases-2', 'Sitting-1', 'Sitting-2', 'SittingDown-1', 'SittingDown-2', 'Smoking-1', 'Smoking-2', 'TakingPhoto-1', 'TakingPhoto-2', 'Waiting-1', 'Waiting-2', 'Walking-1', 'Walking-2', 'WalkingDog-1', 'WalkingDog-2', 'WalkingTogether-1', 'WalkingTogether-2']
148 |
149 |
150 | specific_subject = "S9"
151 | specific_action = "WalkingDog-2"
152 | mask_subject = labels['subject_idx'] == subject_name.index(specific_subject)
153 | actions = [action_name.index(specific_action)]
154 | mask_actions = np.isin(labels['action_idx'], actions)
155 | mask_subject = mask_subject & mask_actions
156 | indices = []
157 | indices.append(np.nonzero(mask_subject)[0])
158 | specific_label = labels[np.concatenate(indices)]
159 | specific_3d_skeleton = specific_label['keypoints']
160 |
161 | specific_camera_config = camera_configs[subject_name.index(specific_subject)]
162 | specific_camera_config = [
163 | Camera(specific_camera_config["R"][i], specific_camera_config["t"][i], specific_camera_config["K"][i]) for i in
164 | range(len(camera_name))]
165 |
166 | # first person setup
167 | yaw = -125
168 | pitch = -15
169 | world_up = np.array([0.0, 1.0, 0.0])
170 | position = np.array([5000, 2500, 7557])
171 | front = np.array([0.0, 0.0, -1.0])
172 | right = np.array([0.0, 0.0, 0.0])
173 |
174 | grid_vertices, grid_color = generate_grid_mesh(-4, 4, step=1)
175 | grid_vertices = grid_vertices.reshape(-1, 3)
176 |
177 | rorate_x_90 = transformations.rotation_matrix(np.radians(-90), (1, 0, 0))
178 |
179 | frame_size = 900
180 | original_video_frame_size = 1000
181 | frame = np.zeros([frame_size, frame_size])
182 |
183 | for i in range(len(camera_name)):
184 | specific_camera_config[i].update_after_resize((original_video_frame_size,) * 2,
185 | (frame_size,) * 2)
186 |
187 | update_camera_vectors()
188 | view_matrix = look_at(position, position + front, world_up)
189 |
190 | projection_matrix = np.array([[2.41421, 0, 0, 0],
191 | [0, 2.41421, 0, 0],
192 | [0, 0, -1, -0.2],
193 | [0, 0, -1, 0]], dtype=np.float32)
194 |
195 | o_view_matrix = view_matrix.copy()
196 | o_projection_matrix = projection_matrix.copy()
197 |
198 | total_frame = specific_3d_skeleton.shape[0]
199 | frame_index = 0
200 |
201 | view_camera_index = -1
202 | while True:
203 | if frame_index == total_frame:
204 | frame_index = 0
205 | frame = np.zeros([frame_size, frame_size, 3])
206 | if view_camera_index >= 0:
207 | view_matrix = None
208 | projection_matrix = specific_camera_config[view_camera_index].projection
209 | else:
210 | view_matrix = o_view_matrix
211 | projection_matrix = o_projection_matrix
212 |
213 | grid_vertices_project = grid_vertices @ (np.eye(3) if view_matrix is None else rorate_x_90[:3, :3].T)
214 | grid_vertices_project = grid_vertices_project @ transformations.scale_matrix(650)[:3, :3].T
215 | grid_vertices_project = projection_to_2d_plane(grid_vertices_project, projection_matrix, view_matrix,
216 | int(frame_size / 2)).reshape(-1, 4)
217 |
218 | # draw line
219 | for index, line in enumerate(grid_vertices_project):
220 | cv2.line(frame, (line[0], line[1]), (line[2], line[3]), grid_color[index].tolist())
221 |
222 | # draw camera
223 | for camera_index, conf in enumerate(specific_camera_config):
224 | if view_camera_index == camera_index:
225 | continue
226 | m_rt = transformations.identity_matrix()
227 | r = np.array(conf.R, dtype=np.float32).T
228 | m_rt[:-1, -1] = -r @ np.array(conf.t, dtype=np.float32).squeeze()
229 | m_rt[:-1, :-1] = r
230 |
231 | m_s = transformations.identity_matrix() * 200
232 | m_s[3, 3] = 1
233 |
234 | camera_vertices_convert = homogeneous_to_euclidean(
235 | euclidean_to_homogeneous(camera_vertices) @ (
236 | (np.eye(4) if view_matrix is None else rorate_x_90) @ m_rt @ m_s).T)
237 |
238 | camera_vertices_convert = projection_to_2d_plane(camera_vertices_convert, projection_matrix, view_matrix,
239 | int(frame_size / 2))
240 | camera_vertices_convert = camera_vertices_convert.reshape(-1, 4)
241 | for index, line in enumerate(camera_vertices_convert):
242 | cv2.line(frame, (line[0], line[1]), (line[2], line[3]), (0, 153, 255), thickness=1)
243 | cv2.putText(frame, camera_name[camera_index],
244 | (camera_vertices_convert[1, 0], camera_vertices_convert[1, 1] - 30),
245 | cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255))
246 |
247 | specific_3d_skeleton_project = specific_3d_skeleton[frame_index].reshape(-1, 3)
248 | specific_3d_skeleton_project = specific_3d_skeleton_project @ (
249 | np.eye(3) if view_matrix is None else rorate_x_90[:3, :3]).T
250 | specific_3d_skeleton_project = specific_3d_skeleton_project @ np.eye(3, dtype=np.float32) * 1
251 | specific_3d_skeleton_project = projection_to_2d_plane(specific_3d_skeleton_project, projection_matrix, view_matrix,
252 | int(frame_size / 2)).reshape(17, 2)
253 | for c in human36m_connectivity_dict:
254 | cv2.line(frame, (*specific_3d_skeleton_project[c[0]],), (*specific_3d_skeleton_project[c[1]],),
255 | (100, 155, 255), thickness=2)
256 | cv2.circle(frame, (*specific_3d_skeleton_project[c[0]],), 3, (0, 0, 255), -1)
257 | cv2.circle(frame, (*specific_3d_skeleton_project[c[1]],), 3, (0, 0, 255), -1)
258 |
259 | frame_index += 1
260 | # print(frame_index)
261 | cv2.imshow(specific_action, frame)
262 | if cv2.waitKey(6) & 0xFF == ord('1'):
263 | view_camera_index += 1
264 | if view_camera_index == 4:
265 | view_camera_index = -1
266 |
267 | if cv2.waitKey(2) & 0xFF == ord('q'):
268 | break
269 |
270 | cv2.destroyAllWindows()
271 |
--------------------------------------------------------------------------------
/least_square.py:
--------------------------------------------------------------------------------
1 | from scipy import optimize
2 | import numpy as np
3 |
4 |
5 | def fit_fun(x):
6 | return 10 + 5 * x
7 |
8 |
9 | x = np.linspace(-2 * np.pi, 2 * np.pi, 100)
10 | y = fit_fun(x) + 0.2 * np.random.rand(1, 100)
11 | y1 = y.reshape(100, )
12 |
13 |
14 | def residuals(p, y, x): #
15 | "计算以p为参数的直线和原始数据之间的误差"
16 | k, b = p
17 | return (k * x + b) - y
18 |
19 |
20 | p_init = np.random.randn(2) # 随机初始化多项式参数
21 | r = optimize.leastsq(residuals, p_init, args=(y1, x))
22 | k, b = r[0]
23 | print("k =", k, "b =", b)
24 |
--------------------------------------------------------------------------------
/per_image_standardization_each_channel.py:
--------------------------------------------------------------------------------
1 | import tensorflow as tf
2 | import numpy as np
3 | import cv2
4 |
5 | origin = cv2.resize(cv2.imread('/home/shuai/Desktop/tool/lena.jpg'), (400, 400))
6 |
7 | a = tf.constant(origin, dtype=tf.float32)
8 |
9 | a = tf.unstack(a, axis=2)
10 | res = []
11 | for i in a:
12 | res.append(tf.squeeze(tf.image.per_image_standardization(tf.expand_dims(i, axis=2)), axis=2))
13 | a = tf.stack(res, axis=2)
14 |
15 | b = tf.image.per_image_standardization(origin)
16 |
17 | with tf.Session() as sess:
18 | sess.run(tf.global_variables_initializer())
19 |
20 | a_, b_ = sess.run([a, b])
21 |
22 | # numpy 分通道标准化图像
23 | image = origin
24 | mean1, mean2, mean3 = np.mean(image[:, :, 0]), np.mean(image[:, :, 1]), np.mean(image[:, :, 2])
25 | std1, std2, std3 = np.std(image[:, :, 0]), np.std(image[:, :, 1]), np.std(image[:, :, 2])
26 | i1 = (image[:, :, 0] - mean1) / std1
27 | i2 = (image[:, :, 1] - mean2) / std2
28 | i3 = (image[:, :, 2] - mean3) / std3
29 | image = np.stack([i1, i2, i3], axis=2)
30 |
31 | cv2.imshow('tf each channel', a_)
32 | cv2.imshow('tf all channel', b_)
33 | cv2.imshow('numpy', image)
34 | cv2.imshow('origin', origin)
35 |
36 | cv2.waitKey(-1)
37 | cv2.destroyAllWindows()
38 |
--------------------------------------------------------------------------------
/resource/gt.npy:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/xing-shuai/PaperDemoAndExampleTest/38887ca5dd3f7b3e57403c5e6f4a3606282f85eb/resource/gt.npy
--------------------------------------------------------------------------------
/resource/h36m_3d_visualization.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/xing-shuai/PaperDemoAndExampleTest/38887ca5dd3f7b3e57403c5e6f4a3606282f85eb/resource/h36m_3d_visualization.gif
--------------------------------------------------------------------------------
/resource/lena.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/xing-shuai/PaperDemoAndExampleTest/38887ca5dd3f7b3e57403c5e6f4a3606282f85eb/resource/lena.jpg
--------------------------------------------------------------------------------
/soft_argmax.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | from HPE_heatmap_generation import CenterGaussianHeatMap
3 |
4 |
5 | def spatial_softmax(img):
6 | exp_img = np.exp(img)
7 | exp_sum = np.sum(exp_img)
8 | return exp_img / exp_sum
9 |
10 |
11 | def soft_argmax(array):
12 | print(np.sum(array))
13 | a = np.random.randn(5, 5)
14 | a = a + -np.min(a)
15 | print(np.unravel_index(np.argmax(a), a.shape))
16 | array = np.exp(a) / np.sum(np.exp(a))
17 | max_position = np.zeros([2])
18 | r, c = np.shape(array)
19 | for i in range(r):
20 | for j in range(c):
21 | max_position += np.array([i, j]) * array[i, j]
22 | return max_position
23 |
24 |
25 | if __name__ == "__main__":
26 | img = np.zeros([100, 20])
27 |
28 | height, width = np.shape(img)
29 | cy, cx = height / 2.0, width / 2.0
30 |
31 | heatmap = CenterGaussianHeatMap(height, width, cx, cy, 2)
32 |
33 | soft_heatmap = spatial_softmax(heatmap)
34 |
35 | print(np.unravel_index(soft_heatmap.argmax(), soft_heatmap.shape))
36 |
37 | print(soft_argmax(soft_heatmap))
38 |
39 | pass
40 |
--------------------------------------------------------------------------------
/test.py:
--------------------------------------------------------------------------------
1 | # # import numpy as np
2 | # #
3 | # # #
4 | # # # # beta = 12
5 | # # # # y_est = np.array([[1.1, 3.0, 1.1, 1.3, 0.8]])
6 | # # # # a = np.exp(beta * y_est)
7 | # # # # b = np.sum(np.exp(beta * y_est))
8 | # # # # softmax = a / b
9 | # # # # max = np.sum(softmax * y_est)
10 | # # # # print(max)
11 | # # # # pos = range(y_est.size)
12 | # # # # print(pos)
13 | # # # # softargmax = np.sum(softmax * pos)
14 | # # # # print(softargmax)
15 | # # #
16 | # # # a = np.random.randn(100, 100) * 50
17 | # # # a = a + -np.min(a)
18 | # # #
19 | # # # a = np.exp(a) / np.sum(np.exp(a))
20 | # # # # print(np.sum(a))
21 | # # # sum_r = np.sum(a, axis=1)
22 | # # # sum_c = np.sum(a, axis=0)
23 | # # #
24 | # # # # print(a)
25 | # # #
26 | # # # # print(sum_c)
27 | # # #
28 | # # # # print(sum_c * np.arange(0, sum_c.shape[0]))
29 | # # #
30 | # # # print(np.sum(sum_c * np.arange(0, sum_c.shape[0])))
31 | # # #
32 | # # # print(np.argmax(a))
33 | # # #
34 | # # # print(np.unravel_index(np.argmax(a), a.shape))
35 | # # #
36 | # # # print(np.sum(np.reshape(a, (a.shape[0] * a.shape[1],)) * np.arange(0, a.shape[0] * a.shape[1])))
37 | # #
38 | # #
39 | # # # import numpy as np
40 | # #
41 | # # # ss = np.zeros([64])
42 | # # # count = 0
43 | # # # cc = []
44 | # # # for i in range(64):
45 | # # # aa = np.random.randint(0, 63)
46 | # # # if aa in cc:
47 | # # # pass
48 | # # # else:
49 | # # # cc.append(aa)
50 | # # #
51 | # # # count += 1
52 | # # #
53 | # # # print(count / 64)
54 | # #
55 | # # m = np.array([[1, 2, 3, 5, 7, 6],
56 | # # [2, 1, 4, 5, 7, 4],
57 | # # [3, 4, 5, 6, 3, 6],
58 | # # [2, 3, 1, 4, 6, 8],
59 | # # [5, 6, 1, 4, 6, 2],
60 | # # [4, 2, 4, 1, 1, 6]])
61 | # #
62 | # # position = (-1, -1)
63 | # # direction = 0
64 | # #
65 | # #
66 | # # def find_ne(p, d):
67 | # # if d == 1:
68 | # # if p[1] >= 4:
69 | # # return ((p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]))
70 | # # else:
71 | # # return ((p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]), (p[0], p[1] + 2, 1, m[p[0], p[1] + 1]))
72 | # # if d == 0:
73 | # # if p[1] <= 1:
74 | # # return ((p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]), (p[0], p[1] - 2, 0, m[p[0], p[1] - 1]))
75 | # # else:
76 | # # return ((p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]))
77 | # # if d == 2:
78 | # # if p[0] >= 4:
79 | # #
80 | # # return ()
81 | # # else:
82 | # # if p[1] <= 1:
83 | # # return ((p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]), (p[0], p[1] + 2, 1, m[p[0], p[1] + 1]))
84 | # # else:
85 | # # if p[1] >= 4:
86 | # # return ((p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]), (p[0], p[1] - 2, 0, m[p[0], p[1] - 1]))
87 | # # else:
88 | # # return (
89 | # # (p[0] + 2, p[1], 2, m[p[0] + 1, p[1]]), (p[0], p[1] - 2, 0, m[p[0], p[1] - 1]),
90 | # # (p[0], p[1] + 2, 1, m[p[0], p[1] + 1]))
91 | # #
92 | # #
93 | # # time = [0]
94 | # # pre_time = 0
95 | #
96 | #
97 | # import cv2 as cv
98 | # import numpy as np
99 | #
100 | #
101 | # def face_detect_demo(image):
102 | # gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
103 | # face_detector = cv.CascadeClassifier("E:/opencv/build/etc/haarcascades/haarcascade_frontalface_alt.xml")
104 | # faces = face_detector.detectMultiScale(gray, 1.1, 2)
105 | # for x, y, w, h in faces:
106 | # cv.rectangle(image, (x, y), (x + w, y + h), (0, 0, 255), 2)
107 | # cv.imshow("result", image)
108 | #
109 | #
110 | # print("--------- Python OpenCV Tutorial ---------")
111 | # capture = cv.VideoCapture(0)
112 | # cv.namedWindow("result", cv.WINDOW_AUTOSIZE)
113 | # while (True):
114 | # ret, frame = capture.read()
115 | # frame = cv.flip(frame, 1)
116 | # face_detect_demo(frame)
117 | # c = cv.waitKey(10)
118 | # if c == 27: # ESC
119 | # break
120 | # cv.waitKey(0)
121 | #
122 | # cv.destroyAllWindows()
123 | # #
124 | # #
125 | # # def step(p, d):
126 | # # global pre_time
127 | # # ne = find_ne(p, d)
128 | # # if len(ne) == 0:
129 | # # time.append(pre_time)
130 | # # for i in ne:
131 | # # pre_time = time[-1]
132 | # # time[-1] += i[3]
133 | # # step((i[0], i[1]), i[2])
134 | # # pass
135 | # # pass
136 | # #
137 | # #
138 | # # for i in range(6):
139 | # # step((0, 0), 1)
140 | # #
141 | # # print(min(time))
142 |
143 |
144 | # import os
145 | #
146 | # ava_ips = []
147 | # allocated_ips = []
148 | # for i in range(2, 255):
149 | # command = os.system('ping -c 1 10.21.243.' + str(i))
150 | # if command == 0:
151 | # allocated_ips.append(i)
152 | # pass # Sucess
153 | # else:
154 | # ava_ips.append(i)
155 | # # print(str(i))
156 | #
157 | # print('allocated_ips', allocated_ips)
158 | # print('ava_ips', ava_ips)
159 |
160 | import numpy as np
161 |
162 | aaaa = [1, 2, [2.0, 4.0], ['sdssds'], 'sdsdsd', np.random.random((100, 100)),
163 | [np.random.random((100, 100)), np.random.random((100, 100))]]
164 | aaaa.remove(aaaa[-1])
165 | print(aaaa)
166 |
--------------------------------------------------------------------------------
/transformations.py:
--------------------------------------------------------------------------------
1 | # -*- coding: utf-8 -*-
2 | # transformations.py
3 |
4 | # Copyright (c) 2006-2017, Christoph Gohlke
5 | # Copyright (c) 2006-2017, The Regents of the University of California
6 | # Produced at the Laboratory for Fluorescence Dynamics
7 | # All rights reserved.
8 | #
9 | # Redistribution and use in source and binary forms, with or without
10 | # modification, are permitted provided that the following conditions are met:
11 | #
12 | # * Redistributions of source code must retain the above copyright
13 | # notice, this list of conditions and the following disclaimer.
14 | # * Redistributions in binary form must reproduce the above copyright
15 | # notice, this list of conditions and the following disclaimer in the
16 | # documentation and/or other materials provided with the distribution.
17 | # * Neither the name of the copyright holders nor the names of any
18 | # contributors may be used to endorse or promote products derived
19 | # from this software without specific prior written permission.
20 | #
21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 | # POSSIBILITY OF SUCH DAMAGE.
32 |
33 | """Homogeneous Transformation Matrices and Quaternions.
34 |
35 | A library for calculating 4x4 matrices for translating, rotating, reflecting,
36 | scaling, shearing, projecting, orthogonalizing, and superimposing arrays of
37 | 3D homogeneous coordinates as well as for converting between rotation matrices,
38 | Euler angles, and quaternions. Also includes an Arcball control object and
39 | functions to decompose transformation matrices.
40 |
41 | :Author:
42 | `Christoph Gohlke `_
43 |
44 | :Organization:
45 | Laboratory for Fluorescence Dynamics, University of California, Irvine
46 |
47 | :Version: 2017.02.17
48 |
49 | Requirements
50 | ------------
51 | * `CPython 2.7 or 3.5 `_
52 | * `Numpy 1.11 `_
53 | * `Transformations.c 2017.02.17 `_
54 | (recommended for speedup of some functions)
55 |
56 | Notes
57 | -----
58 | The API is not stable yet and is expected to change between revisions.
59 |
60 | This Python code is not optimized for speed. Refer to the transformations.c
61 | module for a faster implementation of some functions.
62 |
63 | Documentation in HTML format can be generated with epydoc.
64 |
65 | Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using
66 | numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using
67 | numpy.dot(M, v) for shape (4, \*) column vectors, respectively
68 | numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points").
69 |
70 | This module follows the "column vectors on the right" and "row major storage"
71 | (C contiguous) conventions. The translation components are in the right column
72 | of the transformation matrix, i.e. M[:3, 3].
73 | The transpose of the transformation matrices may have to be used to interface
74 | with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16].
75 |
76 | Calculations are carried out with numpy.float64 precision.
77 |
78 | Vector, point, quaternion, and matrix function arguments are expected to be
79 | "array like", i.e. tuple, list, or numpy arrays.
80 |
81 | Return types are numpy arrays unless specified otherwise.
82 |
83 | Angles are in radians unless specified otherwise.
84 |
85 | Quaternions w+ix+jy+kz are represented as [w, x, y, z].
86 |
87 | A triple of Euler angles can be applied/interpreted in 24 ways, which can
88 | be specified using a 4 character string or encoded 4-tuple:
89 |
90 | *Axes 4-string*: e.g. 'sxyz' or 'ryxy'
91 |
92 | - first character : rotations are applied to 's'tatic or 'r'otating frame
93 | - remaining characters : successive rotation axis 'x', 'y', or 'z'
94 |
95 | *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
96 |
97 | - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
98 | - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed
99 | by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
100 | - repetition : first and last axis are same (1) or different (0).
101 | - frame : rotations are applied to static (0) or rotating (1) frame.
102 |
103 | Other Python packages and modules for 3D transformations and quaternions:
104 |
105 | * `Transforms3d `_
106 | includes most code of this module.
107 | * `Blender.mathutils `_
108 | * `numpy-dtypes `_
109 |
110 | References
111 | ----------
112 | (1) Matrices and transformations. Ronald Goldman.
113 | In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
114 | (2) More matrices and transformations: shear and pseudo-perspective.
115 | Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
116 | (3) Decomposing a matrix into simple transformations. Spencer Thomas.
117 | In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
118 | (4) Recovering the data from the transformation matrix. Ronald Goldman.
119 | In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991.
120 | (5) Euler angle conversion. Ken Shoemake.
121 | In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
122 | (6) Arcball rotation control. Ken Shoemake.
123 | In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
124 | (7) Representing attitude: Euler angles, unit quaternions, and rotation
125 | vectors. James Diebel. 2006.
126 | (8) A discussion of the solution for the best rotation to relate two sets
127 | of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.
128 | (9) Closed-form solution of absolute orientation using unit quaternions.
129 | BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642.
130 | (10) Quaternions. Ken Shoemake.
131 | http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf
132 | (11) From quaternion to matrix and back. JMP van Waveren. 2005.
133 | http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
134 | (12) Uniform random rotations. Ken Shoemake.
135 | In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992.
136 | (13) Quaternion in molecular modeling. CFF Karney.
137 | J Mol Graph Mod, 25(5):595-604
138 | (14) New method for extracting the quaternion from a rotation matrix.
139 | Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087.
140 | (15) Multiple View Geometry in Computer Vision. Hartley and Zissermann.
141 | Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130.
142 | (16) Column Vectors vs. Row Vectors.
143 | http://steve.hollasch.net/cgindex/math/matrix/column-vec.html
144 |
145 | Examples
146 | --------
147 | >>> alpha, beta, gamma = 0.123, -1.234, 2.345
148 | >>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
149 | >>> I = identity_matrix()
150 | >>> Rx = rotation_matrix(alpha, xaxis)
151 | >>> Ry = rotation_matrix(beta, yaxis)
152 | >>> Rz = rotation_matrix(gamma, zaxis)
153 | >>> R = concatenate_matrices(Rx, Ry, Rz)
154 | >>> euler = euler_from_matrix(R, 'rxyz')
155 | >>> numpy.allclose([alpha, beta, gamma], euler)
156 | True
157 | >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
158 | >>> is_same_transform(R, Re)
159 | True
160 | >>> al, be, ga = euler_from_matrix(Re, 'rxyz')
161 | >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
162 | True
163 | >>> qx = quaternion_about_axis(alpha, xaxis)
164 | >>> qy = quaternion_about_axis(beta, yaxis)
165 | >>> qz = quaternion_about_axis(gamma, zaxis)
166 | >>> q = quaternion_multiply(qx, qy)
167 | >>> q = quaternion_multiply(q, qz)
168 | >>> Rq = quaternion_matrix(q)
169 | >>> is_same_transform(R, Rq)
170 | True
171 | >>> S = scale_matrix(1.23, origin)
172 | >>> T = translation_matrix([1, 2, 3])
173 | >>> Z = shear_matrix(beta, xaxis, origin, zaxis)
174 | >>> R = random_rotation_matrix(numpy.random.rand(3))
175 | >>> M = concatenate_matrices(T, R, Z, S)
176 | >>> scale, shear, angles, trans, persp = decompose_matrix(M)
177 | >>> numpy.allclose(scale, 1.23)
178 | True
179 | >>> numpy.allclose(trans, [1, 2, 3])
180 | True
181 | >>> numpy.allclose(shear, [0, math.tan(beta), 0])
182 | True
183 | >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
184 | True
185 | >>> M1 = compose_matrix(scale, shear, angles, trans, persp)
186 | >>> is_same_transform(M, M1)
187 | True
188 | >>> v0, v1 = random_vector(3), random_vector(3)
189 | >>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1))
190 | >>> v2 = numpy.dot(v0, M[:3,:3].T)
191 | >>> numpy.allclose(unit_vector(v1), unit_vector(v2))
192 | True
193 |
194 | """
195 |
196 | from __future__ import division, print_function
197 |
198 | import math
199 |
200 | import numpy
201 |
202 | __version__ = '2017.02.17'
203 | __docformat__ = 'restructuredtext en'
204 | __all__ = ()
205 |
206 |
207 | def identity_matrix():
208 | """Return 4x4 identity/unit matrix.
209 |
210 | >>> I = identity_matrix()
211 | >>> numpy.allclose(I, numpy.dot(I, I))
212 | True
213 | >>> numpy.sum(I), numpy.trace(I)
214 | (4.0, 4.0)
215 | >>> numpy.allclose(I, numpy.identity(4))
216 | True
217 |
218 | """
219 | return numpy.identity(4)
220 |
221 |
222 | def translation_matrix(direction):
223 | """Return matrix to translate by direction vector.
224 |
225 | >>> v = numpy.random.random(3) - 0.5
226 | >>> numpy.allclose(v, translation_matrix(v)[:3, 3])
227 | True
228 |
229 | """
230 | M = numpy.identity(4)
231 | M[:3, 3] = direction[:3]
232 | return M
233 |
234 |
235 | def translation_from_matrix(matrix):
236 | """Return translation vector from translation matrix.
237 |
238 | >>> v0 = numpy.random.random(3) - 0.5
239 | >>> v1 = translation_from_matrix(translation_matrix(v0))
240 | >>> numpy.allclose(v0, v1)
241 | True
242 |
243 | """
244 | return numpy.array(matrix, copy=False)[:3, 3].copy()
245 |
246 |
247 | def reflection_matrix(point, normal):
248 | """Return matrix to mirror at plane defined by point and normal vector.
249 |
250 | >>> v0 = numpy.random.random(4) - 0.5
251 | >>> v0[3] = 1.
252 | >>> v1 = numpy.random.random(3) - 0.5
253 | >>> R = reflection_matrix(v0, v1)
254 | >>> numpy.allclose(2, numpy.trace(R))
255 | True
256 | >>> numpy.allclose(v0, numpy.dot(R, v0))
257 | True
258 | >>> v2 = v0.copy()
259 | >>> v2[:3] += v1
260 | >>> v3 = v0.copy()
261 | >>> v2[:3] -= v1
262 | >>> numpy.allclose(v2, numpy.dot(R, v3))
263 | True
264 |
265 | """
266 | normal = unit_vector(normal[:3])
267 | M = numpy.identity(4)
268 | M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
269 | M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
270 | return M
271 |
272 |
273 | def reflection_from_matrix(matrix):
274 | """Return mirror plane point and normal vector from reflection matrix.
275 |
276 | >>> v0 = numpy.random.random(3) - 0.5
277 | >>> v1 = numpy.random.random(3) - 0.5
278 | >>> M0 = reflection_matrix(v0, v1)
279 | >>> point, normal = reflection_from_matrix(M0)
280 | >>> M1 = reflection_matrix(point, normal)
281 | >>> is_same_transform(M0, M1)
282 | True
283 |
284 | """
285 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)
286 | # normal: unit eigenvector corresponding to eigenvalue -1
287 | w, V = numpy.linalg.eig(M[:3, :3])
288 | i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0]
289 | if not len(i):
290 | raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
291 | normal = numpy.real(V[:, i[0]]).squeeze()
292 | # point: any unit eigenvector corresponding to eigenvalue 1
293 | w, V = numpy.linalg.eig(M)
294 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
295 | if not len(i):
296 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
297 | point = numpy.real(V[:, i[-1]]).squeeze()
298 | point /= point[3]
299 | return point, normal
300 |
301 |
302 | def rotation_matrix(angle, direction, point=None):
303 | """Return matrix to rotate about axis defined by point and direction.
304 |
305 | >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0])
306 | >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])
307 | True
308 | >>> angle = (random.random() - 0.5) * (2*math.pi)
309 | >>> direc = numpy.random.random(3) - 0.5
310 | >>> point = numpy.random.random(3) - 0.5
311 | >>> R0 = rotation_matrix(angle, direc, point)
312 | >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
313 | >>> is_same_transform(R0, R1)
314 | True
315 | >>> R0 = rotation_matrix(angle, direc, point)
316 | >>> R1 = rotation_matrix(-angle, -direc, point)
317 | >>> is_same_transform(R0, R1)
318 | True
319 | >>> I = numpy.identity(4, numpy.float64)
320 | >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
321 | True
322 | >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2,
323 | ... direc, point)))
324 | True
325 |
326 | """
327 | sina = math.sin(angle)
328 | cosa = math.cos(angle)
329 | direction = unit_vector(direction[:3])
330 | # rotation matrix around unit vector
331 | R = numpy.diag([cosa, cosa, cosa])
332 | R += numpy.outer(direction, direction) * (1.0 - cosa)
333 | direction *= sina
334 | R += numpy.array([[0.0, -direction[2], direction[1]],
335 | [direction[2], 0.0, -direction[0]],
336 | [-direction[1], direction[0], 0.0]])
337 | M = numpy.identity(4)
338 | M[:3, :3] = R
339 | if point is not None:
340 | # rotation not around origin
341 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
342 | M[:3, 3] = point - numpy.dot(R, point)
343 | return M
344 |
345 |
346 | def rotation_from_matrix(matrix):
347 | """Return rotation angle and axis from rotation matrix.
348 |
349 | >>> angle = (random.random() - 0.5) * (2*math.pi)
350 | >>> direc = numpy.random.random(3) - 0.5
351 | >>> point = numpy.random.random(3) - 0.5
352 | >>> R0 = rotation_matrix(angle, direc, point)
353 | >>> angle, direc, point = rotation_from_matrix(R0)
354 | >>> R1 = rotation_matrix(angle, direc, point)
355 | >>> is_same_transform(R0, R1)
356 | True
357 |
358 | """
359 | R = numpy.array(matrix, dtype=numpy.float64, copy=False)
360 | R33 = R[:3, :3]
361 | # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
362 | w, W = numpy.linalg.eig(R33.T)
363 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
364 | if not len(i):
365 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
366 | direction = numpy.real(W[:, i[-1]]).squeeze()
367 | # point: unit eigenvector of R33 corresponding to eigenvalue of 1
368 | w, Q = numpy.linalg.eig(R)
369 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
370 | if not len(i):
371 | raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
372 | point = numpy.real(Q[:, i[-1]]).squeeze()
373 | point /= point[3]
374 | # rotation angle depending on direction
375 | cosa = (numpy.trace(R33) - 1.0) / 2.0
376 | if abs(direction[2]) > 1e-8:
377 | sina = (R[1, 0] + (cosa - 1.0) * direction[0] * direction[1]) / direction[2]
378 | elif abs(direction[1]) > 1e-8:
379 | sina = (R[0, 2] + (cosa - 1.0) * direction[0] * direction[2]) / direction[1]
380 | else:
381 | sina = (R[2, 1] + (cosa - 1.0) * direction[1] * direction[2]) / direction[0]
382 | angle = math.atan2(sina, cosa)
383 | return angle, direction, point
384 |
385 |
386 | def scale_matrix(factor, origin=None, direction=None):
387 | """Return matrix to scale by factor around origin in direction.
388 |
389 | Use factor -1 for point symmetry.
390 |
391 | >>> v = (numpy.random.rand(4, 5) - 0.5) * 20
392 | >>> v[3] = 1
393 | >>> S = scale_matrix(-1.234)
394 | >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3])
395 | True
396 | >>> factor = random.random() * 10 - 5
397 | >>> origin = numpy.random.random(3) - 0.5
398 | >>> direct = numpy.random.random(3) - 0.5
399 | >>> S = scale_matrix(factor, origin)
400 | >>> S = scale_matrix(factor, origin, direct)
401 |
402 | """
403 | if direction is None:
404 | # uniform scaling
405 | M = numpy.diag([factor, factor, factor, 1.0])
406 | if origin is not None:
407 | M[:3, 3] = origin[:3]
408 | M[:3, 3] *= 1.0 - factor
409 | else:
410 | # nonuniform scaling
411 | direction = unit_vector(direction[:3])
412 | factor = 1.0 - factor
413 | M = numpy.identity(4)
414 | M[:3, :3] -= factor * numpy.outer(direction, direction)
415 | if origin is not None:
416 | M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction
417 | return M
418 |
419 |
420 | def scale_from_matrix(matrix):
421 | """Return scaling factor, origin and direction from scaling matrix.
422 |
423 | >>> factor = random.random() * 10 - 5
424 | >>> origin = numpy.random.random(3) - 0.5
425 | >>> direct = numpy.random.random(3) - 0.5
426 | >>> S0 = scale_matrix(factor, origin)
427 | >>> factor, origin, direction = scale_from_matrix(S0)
428 | >>> S1 = scale_matrix(factor, origin, direction)
429 | >>> is_same_transform(S0, S1)
430 | True
431 | >>> S0 = scale_matrix(factor, origin, direct)
432 | >>> factor, origin, direction = scale_from_matrix(S0)
433 | >>> S1 = scale_matrix(factor, origin, direction)
434 | >>> is_same_transform(S0, S1)
435 | True
436 |
437 | """
438 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)
439 | M33 = M[:3, :3]
440 | factor = numpy.trace(M33) - 2.0
441 | try:
442 | # direction: unit eigenvector corresponding to eigenvalue factor
443 | w, V = numpy.linalg.eig(M33)
444 | i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0]
445 | direction = numpy.real(V[:, i]).squeeze()
446 | direction /= vector_norm(direction)
447 | except IndexError:
448 | # uniform scaling
449 | factor = (factor + 2.0) / 3.0
450 | direction = None
451 | # origin: any eigenvector corresponding to eigenvalue 1
452 | w, V = numpy.linalg.eig(M)
453 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
454 | if not len(i):
455 | raise ValueError("no eigenvector corresponding to eigenvalue 1")
456 | origin = numpy.real(V[:, i[-1]]).squeeze()
457 | origin /= origin[3]
458 | return factor, origin, direction
459 |
460 |
461 | def projection_matrix(point, normal, direction=None,
462 | perspective=None, pseudo=False):
463 | """Return matrix to project onto plane defined by point and normal.
464 |
465 | Using either perspective point, projection direction, or none of both.
466 |
467 | If pseudo is True, perspective projections will preserve relative depth
468 | such that Perspective = dot(Orthogonal, PseudoPerspective).
469 |
470 | >>> P = projection_matrix([0, 0, 0], [1, 0, 0])
471 | >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:])
472 | True
473 | >>> point = numpy.random.random(3) - 0.5
474 | >>> normal = numpy.random.random(3) - 0.5
475 | >>> direct = numpy.random.random(3) - 0.5
476 | >>> persp = numpy.random.random(3) - 0.5
477 | >>> P0 = projection_matrix(point, normal)
478 | >>> P1 = projection_matrix(point, normal, direction=direct)
479 | >>> P2 = projection_matrix(point, normal, perspective=persp)
480 | >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
481 | >>> is_same_transform(P2, numpy.dot(P0, P3))
482 | True
483 | >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0])
484 | >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20
485 | >>> v0[3] = 1
486 | >>> v1 = numpy.dot(P, v0)
487 | >>> numpy.allclose(v1[1], v0[1])
488 | True
489 | >>> numpy.allclose(v1[0], 3-v1[1])
490 | True
491 |
492 | """
493 | M = numpy.identity(4)
494 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
495 | normal = unit_vector(normal[:3])
496 | if perspective is not None:
497 | # perspective projection
498 | perspective = numpy.array(perspective[:3], dtype=numpy.float64,
499 | copy=False)
500 | M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective - point, normal)
501 | M[:3, :3] -= numpy.outer(perspective, normal)
502 | if pseudo:
503 | # preserve relative depth
504 | M[:3, :3] -= numpy.outer(normal, normal)
505 | M[:3, 3] = numpy.dot(point, normal) * (perspective + normal)
506 | else:
507 | M[:3, 3] = numpy.dot(point, normal) * perspective
508 | M[3, :3] = -normal
509 | M[3, 3] = numpy.dot(perspective, normal)
510 | elif direction is not None:
511 | # parallel projection
512 | direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False)
513 | scale = numpy.dot(direction, normal)
514 | M[:3, :3] -= numpy.outer(direction, normal) / scale
515 | M[:3, 3] = direction * (numpy.dot(point, normal) / scale)
516 | else:
517 | # orthogonal projection
518 | M[:3, :3] -= numpy.outer(normal, normal)
519 | M[:3, 3] = numpy.dot(point, normal) * normal
520 | return M
521 |
522 |
523 | def projection_from_matrix(matrix, pseudo=False):
524 | """Return projection plane and perspective point from projection matrix.
525 |
526 | Return values are same as arguments for projection_matrix function:
527 | point, normal, direction, perspective, and pseudo.
528 |
529 | >>> point = numpy.random.random(3) - 0.5
530 | >>> normal = numpy.random.random(3) - 0.5
531 | >>> direct = numpy.random.random(3) - 0.5
532 | >>> persp = numpy.random.random(3) - 0.5
533 | >>> P0 = projection_matrix(point, normal)
534 | >>> result = projection_from_matrix(P0)
535 | >>> P1 = projection_matrix(*result)
536 | >>> is_same_transform(P0, P1)
537 | True
538 | >>> P0 = projection_matrix(point, normal, direct)
539 | >>> result = projection_from_matrix(P0)
540 | >>> P1 = projection_matrix(*result)
541 | >>> is_same_transform(P0, P1)
542 | True
543 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
544 | >>> result = projection_from_matrix(P0, pseudo=False)
545 | >>> P1 = projection_matrix(*result)
546 | >>> is_same_transform(P0, P1)
547 | True
548 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
549 | >>> result = projection_from_matrix(P0, pseudo=True)
550 | >>> P1 = projection_matrix(*result)
551 | >>> is_same_transform(P0, P1)
552 | True
553 |
554 | """
555 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)
556 | M33 = M[:3, :3]
557 | w, V = numpy.linalg.eig(M)
558 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
559 | if not pseudo and len(i):
560 | # point: any eigenvector corresponding to eigenvalue 1
561 | point = numpy.real(V[:, i[-1]]).squeeze()
562 | point /= point[3]
563 | # direction: unit eigenvector corresponding to eigenvalue 0
564 | w, V = numpy.linalg.eig(M33)
565 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0]
566 | if not len(i):
567 | raise ValueError("no eigenvector corresponding to eigenvalue 0")
568 | direction = numpy.real(V[:, i[0]]).squeeze()
569 | direction /= vector_norm(direction)
570 | # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
571 | w, V = numpy.linalg.eig(M33.T)
572 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0]
573 | if len(i):
574 | # parallel projection
575 | normal = numpy.real(V[:, i[0]]).squeeze()
576 | normal /= vector_norm(normal)
577 | return point, normal, direction, None, False
578 | else:
579 | # orthogonal projection, where normal equals direction vector
580 | return point, direction, None, None, False
581 | else:
582 | # perspective projection
583 | i = numpy.where(abs(numpy.real(w)) > 1e-8)[0]
584 | if not len(i):
585 | raise ValueError(
586 | "no eigenvector not corresponding to eigenvalue 0")
587 | point = numpy.real(V[:, i[-1]]).squeeze()
588 | point /= point[3]
589 | normal = - M[3, :3]
590 | perspective = M[:3, 3] / numpy.dot(point[:3], normal)
591 | if pseudo:
592 | perspective -= normal
593 | return point, normal, None, perspective, pseudo
594 |
595 |
596 | def clip_matrix(left, right, bottom, top, near, far, perspective=False):
597 | """Return matrix to obtain normalized device coordinates from frustum.
598 |
599 | The frustum bounds are axis-aligned along x (left, right),
600 | y (bottom, top) and z (near, far).
601 |
602 | Normalized device coordinates are in range [-1, 1] if coordinates are
603 | inside the frustum.
604 |
605 | If perspective is True the frustum is a truncated pyramid with the
606 | perspective point at origin and direction along z axis, otherwise an
607 | orthographic canonical view volume (a box).
608 |
609 | Homogeneous coordinates transformed by the perspective clip matrix
610 | need to be dehomogenized (divided by w coordinate).
611 |
612 | >>> frustum = numpy.random.rand(6)
613 | >>> frustum[1] += frustum[0]
614 | >>> frustum[3] += frustum[2]
615 | >>> frustum[5] += frustum[4]
616 | >>> M = clip_matrix(perspective=False, *frustum)
617 | >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1])
618 | array([-1., -1., -1., 1.])
619 | >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1])
620 | array([ 1., 1., 1., 1.])
621 | >>> M = clip_matrix(perspective=True, *frustum)
622 | >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1])
623 | >>> v / v[3]
624 | array([-1., -1., -1., 1.])
625 | >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1])
626 | >>> v / v[3]
627 | array([ 1., 1., -1., 1.])
628 |
629 | """
630 | if left >= right or bottom >= top or near >= far:
631 | raise ValueError("invalid frustum")
632 | if perspective:
633 | if near <= _EPS:
634 | raise ValueError("invalid frustum: near <= 0")
635 | t = 2.0 * near
636 | M = [[t / (left - right), 0.0, (right + left) / (right - left), 0.0],
637 | [0.0, t / (bottom - top), (top + bottom) / (top - bottom), 0.0],
638 | [0.0, 0.0, (far + near) / (near - far), t * far / (far - near)],
639 | [0.0, 0.0, -1.0, 0.0]]
640 | else:
641 | M = [[2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)],
642 | [0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)],
643 | [0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)],
644 | [0.0, 0.0, 0.0, 1.0]]
645 | return numpy.array(M)
646 |
647 |
648 | def shear_matrix(angle, direction, point, normal):
649 | """Return matrix to shear by angle along direction vector on shear plane.
650 |
651 | The shear plane is defined by a point and normal vector. The direction
652 | vector must be orthogonal to the plane's normal vector.
653 |
654 | A point P is transformed by the shear matrix into P" such that
655 | the vector P-P" is parallel to the direction vector and its extent is
656 | given by the angle of P-P'-P", where P' is the orthogonal projection
657 | of P onto the shear plane.
658 |
659 | >>> angle = (random.random() - 0.5) * 4*math.pi
660 | >>> direct = numpy.random.random(3) - 0.5
661 | >>> point = numpy.random.random(3) - 0.5
662 | >>> normal = numpy.cross(direct, numpy.random.random(3))
663 | >>> S = shear_matrix(angle, direct, point, normal)
664 | >>> numpy.allclose(1, numpy.linalg.det(S))
665 | True
666 |
667 | """
668 | normal = unit_vector(normal[:3])
669 | direction = unit_vector(direction[:3])
670 | if abs(numpy.dot(normal, direction)) > 1e-6:
671 | raise ValueError("direction and normal vectors are not orthogonal")
672 | angle = math.tan(angle)
673 | M = numpy.identity(4)
674 | M[:3, :3] += angle * numpy.outer(direction, normal)
675 | M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
676 | return M
677 |
678 |
679 | def shear_from_matrix(matrix):
680 | """Return shear angle, direction and plane from shear matrix.
681 |
682 | >>> angle = (random.random() - 0.5) * 4*math.pi
683 | >>> direct = numpy.random.random(3) - 0.5
684 | >>> point = numpy.random.random(3) - 0.5
685 | >>> normal = numpy.cross(direct, numpy.random.random(3))
686 | >>> S0 = shear_matrix(angle, direct, point, normal)
687 | >>> angle, direct, point, normal = shear_from_matrix(S0)
688 | >>> S1 = shear_matrix(angle, direct, point, normal)
689 | >>> is_same_transform(S0, S1)
690 | True
691 |
692 | """
693 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)
694 | M33 = M[:3, :3]
695 | # normal: cross independent eigenvectors corresponding to the eigenvalue 1
696 | w, V = numpy.linalg.eig(M33)
697 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0]
698 | if len(i) < 2:
699 | raise ValueError("no two linear independent eigenvectors found %s" % w)
700 | V = numpy.real(V[:, i]).squeeze().T
701 | lenorm = -1.0
702 | for i0, i1 in ((0, 1), (0, 2), (1, 2)):
703 | n = numpy.cross(V[i0], V[i1])
704 | w = vector_norm(n)
705 | if w > lenorm:
706 | lenorm = w
707 | normal = n
708 | normal /= lenorm
709 | # direction and angle
710 | direction = numpy.dot(M33 - numpy.identity(3), normal)
711 | angle = vector_norm(direction)
712 | direction /= angle
713 | angle = math.atan(angle)
714 | # point: eigenvector corresponding to eigenvalue 1
715 | w, V = numpy.linalg.eig(M)
716 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
717 | if not len(i):
718 | raise ValueError("no eigenvector corresponding to eigenvalue 1")
719 | point = numpy.real(V[:, i[-1]]).squeeze()
720 | point /= point[3]
721 | return angle, direction, point, normal
722 |
723 |
724 | def decompose_matrix(matrix):
725 | """Return sequence of transformations from transformation matrix.
726 |
727 | matrix : array_like
728 | Non-degenerative homogeneous transformation matrix
729 |
730 | Return tuple of:
731 | scale : vector of 3 scaling factors
732 | shear : list of shear factors for x-y, x-z, y-z axes
733 | angles : list of Euler angles about static x, y, z axes
734 | translate : translation vector along x, y, z axes
735 | perspective : perspective partition of matrix
736 |
737 | Raise ValueError if matrix is of wrong type or degenerative.
738 |
739 | >>> T0 = translation_matrix([1, 2, 3])
740 | >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
741 | >>> T1 = translation_matrix(trans)
742 | >>> numpy.allclose(T0, T1)
743 | True
744 | >>> S = scale_matrix(0.123)
745 | >>> scale, shear, angles, trans, persp = decompose_matrix(S)
746 | >>> scale[0]
747 | 0.123
748 | >>> R0 = euler_matrix(1, 2, 3)
749 | >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
750 | >>> R1 = euler_matrix(*angles)
751 | >>> numpy.allclose(R0, R1)
752 | True
753 |
754 | """
755 | M = numpy.array(matrix, dtype=numpy.float64, copy=True).T
756 | if abs(M[3, 3]) < _EPS:
757 | raise ValueError("M[3, 3] is zero")
758 | M /= M[3, 3]
759 | P = M.copy()
760 | P[:, 3] = 0.0, 0.0, 0.0, 1.0
761 | if not numpy.linalg.det(P):
762 | raise ValueError("matrix is singular")
763 |
764 | scale = numpy.zeros((3,))
765 | shear = [0.0, 0.0, 0.0]
766 | angles = [0.0, 0.0, 0.0]
767 |
768 | if any(abs(M[:3, 3]) > _EPS):
769 | perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T))
770 | M[:, 3] = 0.0, 0.0, 0.0, 1.0
771 | else:
772 | perspective = numpy.array([0.0, 0.0, 0.0, 1.0])
773 |
774 | translate = M[3, :3].copy()
775 | M[3, :3] = 0.0
776 |
777 | row = M[:3, :3].copy()
778 | scale[0] = vector_norm(row[0])
779 | row[0] /= scale[0]
780 | shear[0] = numpy.dot(row[0], row[1])
781 | row[1] -= row[0] * shear[0]
782 | scale[1] = vector_norm(row[1])
783 | row[1] /= scale[1]
784 | shear[0] /= scale[1]
785 | shear[1] = numpy.dot(row[0], row[2])
786 | row[2] -= row[0] * shear[1]
787 | shear[2] = numpy.dot(row[1], row[2])
788 | row[2] -= row[1] * shear[2]
789 | scale[2] = vector_norm(row[2])
790 | row[2] /= scale[2]
791 | shear[1:] /= scale[2]
792 |
793 | if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0:
794 | numpy.negative(scale, scale)
795 | numpy.negative(row, row)
796 |
797 | angles[1] = math.asin(-row[0, 2])
798 | if math.cos(angles[1]):
799 | angles[0] = math.atan2(row[1, 2], row[2, 2])
800 | angles[2] = math.atan2(row[0, 1], row[0, 0])
801 | else:
802 | # angles[0] = math.atan2(row[1, 0], row[1, 1])
803 | angles[0] = math.atan2(-row[2, 1], row[1, 1])
804 | angles[2] = 0.0
805 |
806 | return scale, shear, angles, translate, perspective
807 |
808 |
809 | def compose_matrix(scale=None, shear=None, angles=None, translate=None,
810 | perspective=None):
811 | """Return transformation matrix from sequence of transformations.
812 |
813 | This is the inverse of the decompose_matrix function.
814 |
815 | Sequence of transformations:
816 | scale : vector of 3 scaling factors
817 | shear : list of shear factors for x-y, x-z, y-z axes
818 | angles : list of Euler angles about static x, y, z axes
819 | translate : translation vector along x, y, z axes
820 | perspective : perspective partition of matrix
821 |
822 | >>> scale = numpy.random.random(3) - 0.5
823 | >>> shear = numpy.random.random(3) - 0.5
824 | >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi)
825 | >>> trans = numpy.random.random(3) - 0.5
826 | >>> persp = numpy.random.random(4) - 0.5
827 | >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
828 | >>> result = decompose_matrix(M0)
829 | >>> M1 = compose_matrix(*result)
830 | >>> is_same_transform(M0, M1)
831 | True
832 |
833 | """
834 | M = numpy.identity(4)
835 | if perspective is not None:
836 | P = numpy.identity(4)
837 | P[3, :] = perspective[:4]
838 | M = numpy.dot(M, P)
839 | if translate is not None:
840 | T = numpy.identity(4)
841 | T[:3, 3] = translate[:3]
842 | M = numpy.dot(M, T)
843 | if angles is not None:
844 | R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz')
845 | M = numpy.dot(M, R)
846 | if shear is not None:
847 | Z = numpy.identity(4)
848 | Z[1, 2] = shear[2]
849 | Z[0, 2] = shear[1]
850 | Z[0, 1] = shear[0]
851 | M = numpy.dot(M, Z)
852 | if scale is not None:
853 | S = numpy.identity(4)
854 | S[0, 0] = scale[0]
855 | S[1, 1] = scale[1]
856 | S[2, 2] = scale[2]
857 | M = numpy.dot(M, S)
858 | M /= M[3, 3]
859 | return M
860 |
861 |
862 | def orthogonalization_matrix(lengths, angles):
863 | """Return orthogonalization matrix for crystallographic cell coordinates.
864 |
865 | Angles are expected in degrees.
866 |
867 | The de-orthogonalization matrix is the inverse.
868 |
869 | >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
870 | >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
871 | True
872 | >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
873 | >>> numpy.allclose(numpy.sum(O), 43.063229)
874 | True
875 |
876 | """
877 | a, b, c = lengths
878 | angles = numpy.radians(angles)
879 | sina, sinb, _ = numpy.sin(angles)
880 | cosa, cosb, cosg = numpy.cos(angles)
881 | co = (cosa * cosb - cosg) / (sina * sinb)
882 | return numpy.array([
883 | [a * sinb * math.sqrt(1.0 - co * co), 0.0, 0.0, 0.0],
884 | [-a * sinb * co, b * sina, 0.0, 0.0],
885 | [a * cosb, b * cosa, c, 0.0],
886 | [0.0, 0.0, 0.0, 1.0]])
887 |
888 |
889 | def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
890 | """Return affine transform matrix to register two point sets.
891 |
892 | v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous
893 | coordinates, where ndims is the dimensionality of the coordinate space.
894 |
895 | If shear is False, a similarity transformation matrix is returned.
896 | If also scale is False, a rigid/Euclidean transformation matrix
897 | is returned.
898 |
899 | By default the algorithm by Hartley and Zissermann [15] is used.
900 | If usesvd is True, similarity and Euclidean transformation matrices
901 | are calculated by minimizing the weighted sum of squared deviations
902 | (RMSD) according to the algorithm by Kabsch [8].
903 | Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]
904 | is used, which is slower when using this Python implementation.
905 |
906 | The returned matrix performs rotation, translation and uniform scaling
907 | (if specified).
908 |
909 | >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
910 | >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
911 | >>> affine_matrix_from_points(v0, v1)
912 | array([[ 0.14549, 0.00062, 675.50008],
913 | [ 0.00048, 0.14094, 53.24971],
914 | [ 0. , 0. , 1. ]])
915 | >>> T = translation_matrix(numpy.random.random(3)-0.5)
916 | >>> R = random_rotation_matrix(numpy.random.random(3))
917 | >>> S = scale_matrix(random.random())
918 | >>> M = concatenate_matrices(T, R, S)
919 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
920 | >>> v0[3] = 1
921 | >>> v1 = numpy.dot(M, v0)
922 | >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1)
923 | >>> M = affine_matrix_from_points(v0[:3], v1[:3])
924 | >>> numpy.allclose(v1, numpy.dot(M, v0))
925 | True
926 |
927 | More examples in superimposition_matrix()
928 |
929 | """
930 | v0 = numpy.array(v0, dtype=numpy.float64, copy=True)
931 | v1 = numpy.array(v1, dtype=numpy.float64, copy=True)
932 |
933 | ndims = v0.shape[0]
934 | if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
935 | raise ValueError("input arrays are of wrong shape or type")
936 |
937 | # move centroids to origin
938 | t0 = -numpy.mean(v0, axis=1)
939 | M0 = numpy.identity(ndims + 1)
940 | M0[:ndims, ndims] = t0
941 | v0 += t0.reshape(ndims, 1)
942 | t1 = -numpy.mean(v1, axis=1)
943 | M1 = numpy.identity(ndims + 1)
944 | M1[:ndims, ndims] = t1
945 | v1 += t1.reshape(ndims, 1)
946 |
947 | if shear:
948 | # Affine transformation
949 | A = numpy.concatenate((v0, v1), axis=0)
950 | u, s, vh = numpy.linalg.svd(A.T)
951 | vh = vh[:ndims].T
952 | B = vh[:ndims]
953 | C = vh[ndims:2 * ndims]
954 | t = numpy.dot(C, numpy.linalg.pinv(B))
955 | t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1)
956 | M = numpy.vstack((t, ((0.0,) * ndims) + (1.0,)))
957 | elif usesvd or ndims != 3:
958 | # Rigid transformation via SVD of covariance matrix
959 | u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T))
960 | # rotation matrix from SVD orthonormal bases
961 | R = numpy.dot(u, vh)
962 | if numpy.linalg.det(R) < 0.0:
963 | # R does not constitute right handed system
964 | R -= numpy.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)
965 | s[-1] *= -1.0
966 | # homogeneous transformation matrix
967 | M = numpy.identity(ndims + 1)
968 | M[:ndims, :ndims] = R
969 | else:
970 | # Rigid transformation matrix via quaternion
971 | # compute symmetric matrix N
972 | xx, yy, zz = numpy.sum(v0 * v1, axis=1)
973 | xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1)
974 | xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1)
975 | N = [[xx + yy + zz, 0.0, 0.0, 0.0],
976 | [yz - zy, xx - yy - zz, 0.0, 0.0],
977 | [zx - xz, xy + yx, yy - xx - zz, 0.0],
978 | [xy - yx, zx + xz, yz + zy, zz - xx - yy]]
979 | # quaternion: eigenvector corresponding to most positive eigenvalue
980 | w, V = numpy.linalg.eigh(N)
981 | q = V[:, numpy.argmax(w)]
982 | q /= vector_norm(q) # unit quaternion
983 | # homogeneous transformation matrix
984 | M = quaternion_matrix(q)
985 |
986 | if scale and not shear:
987 | # Affine transformation; scale is ratio of RMS deviations from centroid
988 | v0 *= v0
989 | v1 *= v1
990 | M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0))
991 |
992 | # move centroids back
993 | M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0))
994 | M /= M[ndims, ndims]
995 | return M
996 |
997 |
998 | def superimposition_matrix(v0, v1, scale=False, usesvd=True):
999 | """Return matrix to transform given 3D point set into second point set.
1000 |
1001 | v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points.
1002 |
1003 | The parameters scale and usesvd are explained in the more general
1004 | affine_matrix_from_points function.
1005 |
1006 | The returned matrix is a similarity or Euclidean transformation matrix.
1007 | This function has a fast C implementation in transformations.c.
1008 |
1009 | >>> v0 = numpy.random.rand(3, 10)
1010 | >>> M = superimposition_matrix(v0, v0)
1011 | >>> numpy.allclose(M, numpy.identity(4))
1012 | True
1013 | >>> R = random_rotation_matrix(numpy.random.random(3))
1014 | >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]]
1015 | >>> v1 = numpy.dot(R, v0)
1016 | >>> M = superimposition_matrix(v0, v1)
1017 | >>> numpy.allclose(v1, numpy.dot(M, v0))
1018 | True
1019 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
1020 | >>> v0[3] = 1
1021 | >>> v1 = numpy.dot(R, v0)
1022 | >>> M = superimposition_matrix(v0, v1)
1023 | >>> numpy.allclose(v1, numpy.dot(M, v0))
1024 | True
1025 | >>> S = scale_matrix(random.random())
1026 | >>> T = translation_matrix(numpy.random.random(3)-0.5)
1027 | >>> M = concatenate_matrices(T, R, S)
1028 | >>> v1 = numpy.dot(M, v0)
1029 | >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1)
1030 | >>> M = superimposition_matrix(v0, v1, scale=True)
1031 | >>> numpy.allclose(v1, numpy.dot(M, v0))
1032 | True
1033 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1034 | >>> numpy.allclose(v1, numpy.dot(M, v0))
1035 | True
1036 | >>> v = numpy.empty((4, 100, 3))
1037 | >>> v[:, :, 0] = v0
1038 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1039 | >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0]))
1040 | True
1041 |
1042 | """
1043 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3]
1044 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3]
1045 | return affine_matrix_from_points(v0, v1, shear=False,
1046 | scale=scale, usesvd=usesvd)
1047 |
1048 |
1049 | def euler_matrix(ai, aj, ak, axes='sxyz'):
1050 | """Return homogeneous rotation matrix from Euler angles and axis sequence.
1051 |
1052 | ai, aj, ak : Euler's roll, pitch and yaw angles
1053 | axes : One of 24 axis sequences as string or encoded tuple
1054 |
1055 | >>> R = euler_matrix(1, 2, 3, 'syxz')
1056 | >>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
1057 | True
1058 | >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
1059 | >>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
1060 | True
1061 | >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5)
1062 | >>> for axes in _AXES2TUPLE.keys():
1063 | ... R = euler_matrix(ai, aj, ak, axes)
1064 | >>> for axes in _TUPLE2AXES.keys():
1065 | ... R = euler_matrix(ai, aj, ak, axes)
1066 |
1067 | """
1068 | try:
1069 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
1070 | except (AttributeError, KeyError):
1071 | _TUPLE2AXES[axes] # validation
1072 | firstaxis, parity, repetition, frame = axes
1073 |
1074 | i = firstaxis
1075 | j = _NEXT_AXIS[i + parity]
1076 | k = _NEXT_AXIS[i - parity + 1]
1077 |
1078 | if frame:
1079 | ai, ak = ak, ai
1080 | if parity:
1081 | ai, aj, ak = -ai, -aj, -ak
1082 |
1083 | si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
1084 | ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
1085 | cc, cs = ci * ck, ci * sk
1086 | sc, ss = si * ck, si * sk
1087 |
1088 | M = numpy.identity(4)
1089 | if repetition:
1090 | M[i, i] = cj
1091 | M[i, j] = sj * si
1092 | M[i, k] = sj * ci
1093 | M[j, i] = sj * sk
1094 | M[j, j] = -cj * ss + cc
1095 | M[j, k] = -cj * cs - sc
1096 | M[k, i] = -sj * ck
1097 | M[k, j] = cj * sc + cs
1098 | M[k, k] = cj * cc - ss
1099 | else:
1100 | M[i, i] = cj * ck
1101 | M[i, j] = sj * sc - cs
1102 | M[i, k] = sj * cc + ss
1103 | M[j, i] = cj * sk
1104 | M[j, j] = sj * ss + cc
1105 | M[j, k] = sj * cs - sc
1106 | M[k, i] = -sj
1107 | M[k, j] = cj * si
1108 | M[k, k] = cj * ci
1109 | return M
1110 |
1111 |
1112 | def euler_from_matrix(matrix, axes='sxyz'):
1113 | """Return Euler angles from rotation matrix for specified axis sequence.
1114 |
1115 | axes : One of 24 axis sequences as string or encoded tuple
1116 |
1117 | Note that many Euler angle triplets can describe one matrix.
1118 |
1119 | >>> R0 = euler_matrix(1, 2, 3, 'syxz')
1120 | >>> al, be, ga = euler_from_matrix(R0, 'syxz')
1121 | >>> R1 = euler_matrix(al, be, ga, 'syxz')
1122 | >>> numpy.allclose(R0, R1)
1123 | True
1124 | >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5)
1125 | >>> for axes in _AXES2TUPLE.keys():
1126 | ... R0 = euler_matrix(axes=axes, *angles)
1127 | ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
1128 | ... if not numpy.allclose(R0, R1): print(axes, "failed")
1129 |
1130 | """
1131 | try:
1132 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1133 | except (AttributeError, KeyError):
1134 | _TUPLE2AXES[axes] # validation
1135 | firstaxis, parity, repetition, frame = axes
1136 |
1137 | i = firstaxis
1138 | j = _NEXT_AXIS[i + parity]
1139 | k = _NEXT_AXIS[i - parity + 1]
1140 |
1141 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]
1142 | if repetition:
1143 | sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
1144 | if sy > _EPS:
1145 | ax = math.atan2(M[i, j], M[i, k])
1146 | ay = math.atan2(sy, M[i, i])
1147 | az = math.atan2(M[j, i], -M[k, i])
1148 | else:
1149 | ax = math.atan2(-M[j, k], M[j, j])
1150 | ay = math.atan2(sy, M[i, i])
1151 | az = 0.0
1152 | else:
1153 | cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
1154 | if cy > _EPS:
1155 | ax = math.atan2(M[k, j], M[k, k])
1156 | ay = math.atan2(-M[k, i], cy)
1157 | az = math.atan2(M[j, i], M[i, i])
1158 | else:
1159 | ax = math.atan2(-M[j, k], M[j, j])
1160 | ay = math.atan2(-M[k, i], cy)
1161 | az = 0.0
1162 |
1163 | if parity:
1164 | ax, ay, az = -ax, -ay, -az
1165 | if frame:
1166 | ax, az = az, ax
1167 | return ax, ay, az
1168 |
1169 |
1170 | def euler_from_quaternion(quaternion, axes='sxyz'):
1171 | """Return Euler angles from quaternion for specified axis sequence.
1172 |
1173 | >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
1174 | >>> numpy.allclose(angles, [0.123, 0, 0])
1175 | True
1176 |
1177 | """
1178 | return euler_from_matrix(quaternion_matrix(quaternion), axes)
1179 |
1180 |
1181 | def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
1182 | """Return quaternion from Euler angles and axis sequence.
1183 |
1184 | ai, aj, ak : Euler's roll, pitch and yaw angles
1185 | axes : One of 24 axis sequences as string or encoded tuple
1186 |
1187 | >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
1188 | >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
1189 | True
1190 |
1191 | """
1192 | try:
1193 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1194 | except (AttributeError, KeyError):
1195 | _TUPLE2AXES[axes] # validation
1196 | firstaxis, parity, repetition, frame = axes
1197 |
1198 | i = firstaxis + 1
1199 | j = _NEXT_AXIS[i + parity - 1] + 1
1200 | k = _NEXT_AXIS[i - parity] + 1
1201 |
1202 | if frame:
1203 | ai, ak = ak, ai
1204 | if parity:
1205 | aj = -aj
1206 |
1207 | ai /= 2.0
1208 | aj /= 2.0
1209 | ak /= 2.0
1210 | ci = math.cos(ai)
1211 | si = math.sin(ai)
1212 | cj = math.cos(aj)
1213 | sj = math.sin(aj)
1214 | ck = math.cos(ak)
1215 | sk = math.sin(ak)
1216 | cc = ci * ck
1217 | cs = ci * sk
1218 | sc = si * ck
1219 | ss = si * sk
1220 |
1221 | q = numpy.empty((4,))
1222 | if repetition:
1223 | q[0] = cj * (cc - ss)
1224 | q[i] = cj * (cs + sc)
1225 | q[j] = sj * (cc + ss)
1226 | q[k] = sj * (cs - sc)
1227 | else:
1228 | q[0] = cj * cc + sj * ss
1229 | q[i] = cj * sc - sj * cs
1230 | q[j] = cj * ss + sj * cc
1231 | q[k] = cj * cs - sj * sc
1232 | if parity:
1233 | q[j] *= -1.0
1234 |
1235 | return q
1236 |
1237 |
1238 | def quaternion_about_axis(angle, axis):
1239 | """Return quaternion for rotation about axis.
1240 |
1241 | >>> q = quaternion_about_axis(0.123, [1, 0, 0])
1242 | >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0])
1243 | True
1244 |
1245 | """
1246 | q = numpy.array([0.0, axis[0], axis[1], axis[2]])
1247 | qlen = vector_norm(q)
1248 | if qlen > _EPS:
1249 | q *= math.sin(angle / 2.0) / qlen
1250 | q[0] = math.cos(angle / 2.0)
1251 | return q
1252 |
1253 |
1254 | def quaternion_matrix(quaternion):
1255 | """Return homogeneous rotation matrix from quaternion.
1256 |
1257 | >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
1258 | >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
1259 | True
1260 | >>> M = quaternion_matrix([1, 0, 0, 0])
1261 | >>> numpy.allclose(M, numpy.identity(4))
1262 | True
1263 | >>> M = quaternion_matrix([0, 1, 0, 0])
1264 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
1265 | True
1266 |
1267 | """
1268 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1269 | n = numpy.dot(q, q)
1270 | if n < _EPS:
1271 | return numpy.identity(4)
1272 | q *= math.sqrt(2.0 / n)
1273 | q = numpy.outer(q, q)
1274 | return numpy.array([
1275 | [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0],
1276 | [q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0],
1277 | [q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
1278 | [0.0, 0.0, 0.0, 1.0]])
1279 |
1280 |
1281 | def quaternion_from_matrix(matrix, isprecise=False):
1282 | """Return quaternion from rotation matrix.
1283 |
1284 | If isprecise is True, the input matrix is assumed to be a precise rotation
1285 | matrix and a faster algorithm is used.
1286 |
1287 | >>> q = quaternion_from_matrix(numpy.identity(4), True)
1288 | >>> numpy.allclose(q, [1, 0, 0, 0])
1289 | True
1290 | >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1]))
1291 | >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0])
1292 | True
1293 | >>> R = rotation_matrix(0.123, (1, 2, 3))
1294 | >>> q = quaternion_from_matrix(R, True)
1295 | >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
1296 | True
1297 | >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
1298 | ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
1299 | >>> q = quaternion_from_matrix(R)
1300 | >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
1301 | True
1302 | >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
1303 | ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
1304 | >>> q = quaternion_from_matrix(R)
1305 | >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
1306 | True
1307 | >>> R = random_rotation_matrix()
1308 | >>> q = quaternion_from_matrix(R)
1309 | >>> is_same_transform(R, quaternion_matrix(q))
1310 | True
1311 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1312 | ... quaternion_from_matrix(R, isprecise=True))
1313 | True
1314 | >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0)
1315 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1316 | ... quaternion_from_matrix(R, isprecise=True))
1317 | True
1318 |
1319 | """
1320 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
1321 | if isprecise:
1322 | q = numpy.empty((4,))
1323 | t = numpy.trace(M)
1324 | if t > M[3, 3]:
1325 | q[0] = t
1326 | q[3] = M[1, 0] - M[0, 1]
1327 | q[2] = M[0, 2] - M[2, 0]
1328 | q[1] = M[2, 1] - M[1, 2]
1329 | else:
1330 | i, j, k = 0, 1, 2
1331 | if M[1, 1] > M[0, 0]:
1332 | i, j, k = 1, 2, 0
1333 | if M[2, 2] > M[i, i]:
1334 | i, j, k = 2, 0, 1
1335 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
1336 | q[i] = t
1337 | q[j] = M[i, j] + M[j, i]
1338 | q[k] = M[k, i] + M[i, k]
1339 | q[3] = M[k, j] - M[j, k]
1340 | q = q[[3, 0, 1, 2]]
1341 | q *= 0.5 / math.sqrt(t * M[3, 3])
1342 | else:
1343 | m00 = M[0, 0]
1344 | m01 = M[0, 1]
1345 | m02 = M[0, 2]
1346 | m10 = M[1, 0]
1347 | m11 = M[1, 1]
1348 | m12 = M[1, 2]
1349 | m20 = M[2, 0]
1350 | m21 = M[2, 1]
1351 | m22 = M[2, 2]
1352 | # symmetric matrix K
1353 | K = numpy.array([[m00 - m11 - m22, 0.0, 0.0, 0.0],
1354 | [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
1355 | [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
1356 | [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
1357 | K /= 3.0
1358 | # quaternion is eigenvector of K that corresponds to largest eigenvalue
1359 | w, V = numpy.linalg.eigh(K)
1360 | q = V[[3, 0, 1, 2], numpy.argmax(w)]
1361 | if q[0] < 0.0:
1362 | numpy.negative(q, q)
1363 | return q
1364 |
1365 |
1366 | def quaternion_multiply(quaternion1, quaternion0):
1367 | """Return multiplication of two quaternions.
1368 |
1369 | >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
1370 | >>> numpy.allclose(q, [28, -44, -14, 48])
1371 | True
1372 |
1373 | """
1374 | w0, x0, y0, z0 = quaternion0
1375 | w1, x1, y1, z1 = quaternion1
1376 | return numpy.array([
1377 | -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
1378 | x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
1379 | -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
1380 | x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=numpy.float64)
1381 |
1382 |
1383 | def quaternion_conjugate(quaternion):
1384 | """Return conjugate of quaternion.
1385 |
1386 | >>> q0 = random_quaternion()
1387 | >>> q1 = quaternion_conjugate(q0)
1388 | >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:])
1389 | True
1390 |
1391 | """
1392 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1393 | numpy.negative(q[1:], q[1:])
1394 | return q
1395 |
1396 |
1397 | def quaternion_inverse(quaternion):
1398 | """Return inverse of quaternion.
1399 |
1400 | >>> q0 = random_quaternion()
1401 | >>> q1 = quaternion_inverse(q0)
1402 | >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])
1403 | True
1404 |
1405 | """
1406 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1407 | numpy.negative(q[1:], q[1:])
1408 | return q / numpy.dot(q, q)
1409 |
1410 |
1411 | def quaternion_real(quaternion):
1412 | """Return real part of quaternion.
1413 |
1414 | >>> quaternion_real([3, 0, 1, 2])
1415 | 3.0
1416 |
1417 | """
1418 | return float(quaternion[0])
1419 |
1420 |
1421 | def quaternion_imag(quaternion):
1422 | """Return imaginary part of quaternion.
1423 |
1424 | >>> quaternion_imag([3, 0, 1, 2])
1425 | array([ 0., 1., 2.])
1426 |
1427 | """
1428 | return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True)
1429 |
1430 |
1431 | def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
1432 | """Return spherical linear interpolation between two quaternions.
1433 |
1434 | >>> q0 = random_quaternion()
1435 | >>> q1 = random_quaternion()
1436 | >>> q = quaternion_slerp(q0, q1, 0)
1437 | >>> numpy.allclose(q, q0)
1438 | True
1439 | >>> q = quaternion_slerp(q0, q1, 1, 1)
1440 | >>> numpy.allclose(q, q1)
1441 | True
1442 | >>> q = quaternion_slerp(q0, q1, 0.5)
1443 | >>> angle = math.acos(numpy.dot(q0, q))
1444 | >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \
1445 | numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle)
1446 | True
1447 |
1448 | """
1449 | q0 = unit_vector(quat0[:4])
1450 | q1 = unit_vector(quat1[:4])
1451 | if fraction == 0.0:
1452 | return q0
1453 | elif fraction == 1.0:
1454 | return q1
1455 | d = numpy.dot(q0, q1)
1456 | if abs(abs(d) - 1.0) < _EPS:
1457 | return q0
1458 | if shortestpath and d < 0.0:
1459 | # invert rotation
1460 | d = -d
1461 | numpy.negative(q1, q1)
1462 | angle = math.acos(d) + spin * math.pi
1463 | if abs(angle) < _EPS:
1464 | return q0
1465 | isin = 1.0 / math.sin(angle)
1466 | q0 *= math.sin((1.0 - fraction) * angle) * isin
1467 | q1 *= math.sin(fraction * angle) * isin
1468 | q0 += q1
1469 | return q0
1470 |
1471 |
1472 | def random_quaternion(rand=None):
1473 | """Return uniform random unit quaternion.
1474 |
1475 | rand: array like or None
1476 | Three independent random variables that are uniformly distributed
1477 | between 0 and 1.
1478 |
1479 | >>> q = random_quaternion()
1480 | >>> numpy.allclose(1, vector_norm(q))
1481 | True
1482 | >>> q = random_quaternion(numpy.random.random(3))
1483 | >>> len(q.shape), q.shape[0]==4
1484 | (1, True)
1485 |
1486 | """
1487 | if rand is None:
1488 | rand = numpy.random.rand(3)
1489 | else:
1490 | assert len(rand) == 3
1491 | r1 = numpy.sqrt(1.0 - rand[0])
1492 | r2 = numpy.sqrt(rand[0])
1493 | pi2 = math.pi * 2.0
1494 | t1 = pi2 * rand[1]
1495 | t2 = pi2 * rand[2]
1496 | return numpy.array([numpy.cos(t2) * r2, numpy.sin(t1) * r1,
1497 | numpy.cos(t1) * r1, numpy.sin(t2) * r2])
1498 |
1499 |
1500 | def random_rotation_matrix(rand=None):
1501 | """Return uniform random rotation matrix.
1502 |
1503 | rand: array like
1504 | Three independent random variables that are uniformly distributed
1505 | between 0 and 1 for each returned quaternion.
1506 |
1507 | >>> R = random_rotation_matrix()
1508 | >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
1509 | True
1510 |
1511 | """
1512 | return quaternion_matrix(random_quaternion(rand))
1513 |
1514 |
1515 | class Arcball(object):
1516 | """Virtual Trackball Control.
1517 |
1518 | >>> ball = Arcball()
1519 | >>> ball = Arcball(initial=numpy.identity(4))
1520 | >>> ball.place([320, 320], 320)
1521 | >>> ball.down([500, 250])
1522 | >>> ball.drag([475, 275])
1523 | >>> R = ball.matrix()
1524 | >>> numpy.allclose(numpy.sum(R), 3.90583455)
1525 | True
1526 | >>> ball = Arcball(initial=[1, 0, 0, 0])
1527 | >>> ball.place([320, 320], 320)
1528 | >>> ball.setaxes([1, 1, 0], [-1, 1, 0])
1529 | >>> ball.constrain = True
1530 | >>> ball.down([400, 200])
1531 | >>> ball.drag([200, 400])
1532 | >>> R = ball.matrix()
1533 | >>> numpy.allclose(numpy.sum(R), 0.2055924)
1534 | True
1535 | >>> ball.next()
1536 |
1537 | """
1538 |
1539 | def __init__(self, initial=None):
1540 | """Initialize virtual trackball control.
1541 |
1542 | initial : quaternion or rotation matrix
1543 |
1544 | """
1545 | self._axis = None
1546 | self._axes = None
1547 | self._radius = 1.0
1548 | self._center = [0.0, 0.0]
1549 | self._vdown = numpy.array([0.0, 0.0, 1.0])
1550 | self._constrain = False
1551 | if initial is None:
1552 | self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0])
1553 | else:
1554 | initial = numpy.array(initial, dtype=numpy.float64)
1555 | if initial.shape == (4, 4):
1556 | self._qdown = quaternion_from_matrix(initial)
1557 | elif initial.shape == (4,):
1558 | initial /= vector_norm(initial)
1559 | self._qdown = initial
1560 | else:
1561 | raise ValueError("initial not a quaternion or matrix")
1562 | self._qnow = self._qpre = self._qdown
1563 |
1564 | def place(self, center, radius):
1565 | """Place Arcball, e.g. when window size changes.
1566 |
1567 | center : sequence[2]
1568 | Window coordinates of trackball center.
1569 | radius : float
1570 | Radius of trackball in window coordinates.
1571 |
1572 | """
1573 | self._radius = float(radius)
1574 | self._center[0] = center[0]
1575 | self._center[1] = center[1]
1576 |
1577 | def setaxes(self, *axes):
1578 | """Set axes to constrain rotations."""
1579 | if axes is None:
1580 | self._axes = None
1581 | else:
1582 | self._axes = [unit_vector(axis) for axis in axes]
1583 |
1584 | @property
1585 | def constrain(self):
1586 | """Return state of constrain to axis mode."""
1587 | return self._constrain
1588 |
1589 | @constrain.setter
1590 | def constrain(self, value):
1591 | """Set state of constrain to axis mode."""
1592 | self._constrain = bool(value)
1593 |
1594 | def down(self, point):
1595 | """Set initial cursor window coordinates and pick constrain-axis."""
1596 | self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
1597 | self._qdown = self._qpre = self._qnow
1598 | if self._constrain and self._axes is not None:
1599 | self._axis = arcball_nearest_axis(self._vdown, self._axes)
1600 | self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
1601 | else:
1602 | self._axis = None
1603 |
1604 | def drag(self, point):
1605 | """Update current cursor window coordinates."""
1606 | vnow = arcball_map_to_sphere(point, self._center, self._radius)
1607 | if self._axis is not None:
1608 | vnow = arcball_constrain_to_axis(vnow, self._axis)
1609 | self._qpre = self._qnow
1610 | t = numpy.cross(self._vdown, vnow)
1611 | if numpy.dot(t, t) < _EPS:
1612 | self._qnow = self._qdown
1613 | else:
1614 | q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]]
1615 | self._qnow = quaternion_multiply(q, self._qdown)
1616 |
1617 | def next(self, acceleration=0.0):
1618 | """Continue rotation in direction of last drag."""
1619 | q = quaternion_slerp(self._qpre, self._qnow, 2.0 + acceleration, False)
1620 | self._qpre, self._qnow = self._qnow, q
1621 |
1622 | def matrix(self):
1623 | """Return homogeneous rotation matrix."""
1624 | return quaternion_matrix(self._qnow)
1625 |
1626 |
1627 | def arcball_map_to_sphere(point, center, radius):
1628 | """Return unit sphere coordinates from window coordinates."""
1629 | v0 = (point[0] - center[0]) / radius
1630 | v1 = (center[1] - point[1]) / radius
1631 | n = v0 * v0 + v1 * v1
1632 | if n > 1.0:
1633 | # position outside of sphere
1634 | n = math.sqrt(n)
1635 | return numpy.array([v0 / n, v1 / n, 0.0])
1636 | else:
1637 | return numpy.array([v0, v1, math.sqrt(1.0 - n)])
1638 |
1639 |
1640 | def arcball_constrain_to_axis(point, axis):
1641 | """Return sphere point perpendicular to axis."""
1642 | v = numpy.array(point, dtype=numpy.float64, copy=True)
1643 | a = numpy.array(axis, dtype=numpy.float64, copy=True)
1644 | v -= a * numpy.dot(a, v) # on plane
1645 | n = vector_norm(v)
1646 | if n > _EPS:
1647 | if v[2] < 0.0:
1648 | numpy.negative(v, v)
1649 | v /= n
1650 | return v
1651 | if a[2] == 1.0:
1652 | return numpy.array([1.0, 0.0, 0.0])
1653 | return unit_vector([-a[1], a[0], 0.0])
1654 |
1655 |
1656 | def arcball_nearest_axis(point, axes):
1657 | """Return axis, which arc is nearest to point."""
1658 | point = numpy.array(point, dtype=numpy.float64, copy=False)
1659 | nearest = None
1660 | mx = -1.0
1661 | for axis in axes:
1662 | t = numpy.dot(arcball_constrain_to_axis(point, axis), point)
1663 | if t > mx:
1664 | nearest = axis
1665 | mx = t
1666 | return nearest
1667 |
1668 |
1669 | # epsilon for testing whether a number is close to zero
1670 | _EPS = numpy.finfo(float).eps * 4.0
1671 |
1672 | # axis sequences for Euler angles
1673 | _NEXT_AXIS = [1, 2, 0, 1]
1674 |
1675 | # map axes strings to/from tuples of inner axis, parity, repetition, frame
1676 | _AXES2TUPLE = {
1677 | 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
1678 | 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
1679 | 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
1680 | 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
1681 | 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
1682 | 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
1683 | 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
1684 | 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
1685 |
1686 | _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
1687 |
1688 |
1689 | def vector_norm(data, axis=None, out=None):
1690 | """Return length, i.e. Euclidean norm, of ndarray along axis.
1691 |
1692 | >>> v = numpy.random.random(3)
1693 | >>> n = vector_norm(v)
1694 | >>> numpy.allclose(n, numpy.linalg.norm(v))
1695 | True
1696 | >>> v = numpy.random.rand(6, 5, 3)
1697 | >>> n = vector_norm(v, axis=-1)
1698 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
1699 | True
1700 | >>> n = vector_norm(v, axis=1)
1701 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1702 | True
1703 | >>> v = numpy.random.rand(5, 4, 3)
1704 | >>> n = numpy.empty((5, 3))
1705 | >>> vector_norm(v, axis=1, out=n)
1706 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1707 | True
1708 | >>> vector_norm([])
1709 | 0.0
1710 | >>> vector_norm([1])
1711 | 1.0
1712 |
1713 | """
1714 | data = numpy.array(data, dtype=numpy.float64, copy=True)
1715 | if out is None:
1716 | if data.ndim == 1:
1717 | return math.sqrt(numpy.dot(data, data))
1718 | data *= data
1719 | out = numpy.atleast_1d(numpy.sum(data, axis=axis))
1720 | numpy.sqrt(out, out)
1721 | return out
1722 | else:
1723 | data *= data
1724 | numpy.sum(data, axis=axis, out=out)
1725 | numpy.sqrt(out, out)
1726 |
1727 |
1728 | def unit_vector(data, axis=None, out=None):
1729 | """Return ndarray normalized by length, i.e. Euclidean norm, along axis.
1730 |
1731 | >>> v0 = numpy.random.random(3)
1732 | >>> v1 = unit_vector(v0)
1733 | >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
1734 | True
1735 | >>> v0 = numpy.random.rand(5, 4, 3)
1736 | >>> v1 = unit_vector(v0, axis=-1)
1737 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
1738 | >>> numpy.allclose(v1, v2)
1739 | True
1740 | >>> v1 = unit_vector(v0, axis=1)
1741 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
1742 | >>> numpy.allclose(v1, v2)
1743 | True
1744 | >>> v1 = numpy.empty((5, 4, 3))
1745 | >>> unit_vector(v0, axis=1, out=v1)
1746 | >>> numpy.allclose(v1, v2)
1747 | True
1748 | >>> list(unit_vector([]))
1749 | []
1750 | >>> list(unit_vector([1]))
1751 | [1.0]
1752 |
1753 | """
1754 | if out is None:
1755 | data = numpy.array(data, dtype=numpy.float64, copy=True)
1756 | if data.ndim == 1:
1757 | data /= math.sqrt(numpy.dot(data, data))
1758 | return data
1759 | else:
1760 | if out is not data:
1761 | out[:] = numpy.array(data, copy=False)
1762 | data = out
1763 | length = numpy.atleast_1d(numpy.sum(data * data, axis))
1764 | numpy.sqrt(length, length)
1765 | if axis is not None:
1766 | length = numpy.expand_dims(length, axis)
1767 | data /= length
1768 | if out is None:
1769 | return data
1770 |
1771 |
1772 | def random_vector(size):
1773 | """Return array of random doubles in the half-open interval [0.0, 1.0).
1774 |
1775 | >>> v = random_vector(10000)
1776 | >>> numpy.all(v >= 0) and numpy.all(v < 1)
1777 | True
1778 | >>> v0 = random_vector(10)
1779 | >>> v1 = random_vector(10)
1780 | >>> numpy.any(v0 == v1)
1781 | False
1782 |
1783 | """
1784 | return numpy.random.random(size)
1785 |
1786 |
1787 | def vector_product(v0, v1, axis=0):
1788 | """Return vector perpendicular to vectors.
1789 |
1790 | >>> v = vector_product([2, 0, 0], [0, 3, 0])
1791 | >>> numpy.allclose(v, [0, 0, 6])
1792 | True
1793 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1794 | >>> v1 = [[3], [0], [0]]
1795 | >>> v = vector_product(v0, v1)
1796 | >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])
1797 | True
1798 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1799 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1800 | >>> v = vector_product(v0, v1, axis=1)
1801 | >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])
1802 | True
1803 |
1804 | """
1805 | return numpy.cross(v0, v1, axis=axis)
1806 |
1807 |
1808 | def angle_between_vectors(v0, v1, directed=True, axis=0):
1809 | """Return angle between vectors.
1810 |
1811 | If directed is False, the input vectors are interpreted as undirected axes,
1812 | i.e. the maximum angle is pi/2.
1813 |
1814 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])
1815 | >>> numpy.allclose(a, math.pi)
1816 | True
1817 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)
1818 | >>> numpy.allclose(a, 0)
1819 | True
1820 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1821 | >>> v1 = [[3], [0], [0]]
1822 | >>> a = angle_between_vectors(v0, v1)
1823 | >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532])
1824 | True
1825 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1826 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1827 | >>> a = angle_between_vectors(v0, v1, axis=1)
1828 | >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])
1829 | True
1830 |
1831 | """
1832 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False)
1833 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False)
1834 | dot = numpy.sum(v0 * v1, axis=axis)
1835 | dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)
1836 | return numpy.arccos(dot if directed else numpy.fabs(dot))
1837 |
1838 |
1839 | def inverse_matrix(matrix):
1840 | """Return inverse of square transformation matrix.
1841 |
1842 | >>> M0 = random_rotation_matrix()
1843 | >>> M1 = inverse_matrix(M0.T)
1844 | >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))
1845 | True
1846 | >>> for size in range(1, 7):
1847 | ... M0 = numpy.random.rand(size, size)
1848 | ... M1 = inverse_matrix(M0)
1849 | ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size)
1850 |
1851 | """
1852 | return numpy.linalg.inv(matrix)
1853 |
1854 |
1855 | def concatenate_matrices(*matrices):
1856 | """Return concatenation of series of transformation matrices.
1857 |
1858 | >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5
1859 | >>> numpy.allclose(M, concatenate_matrices(M))
1860 | True
1861 | >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))
1862 | True
1863 |
1864 | """
1865 | M = numpy.identity(4)
1866 | for i in matrices:
1867 | M = numpy.dot(M, i)
1868 | return M
1869 |
1870 |
1871 | def is_same_transform(matrix0, matrix1):
1872 | """Return True if two matrices perform same transformation.
1873 |
1874 | >>> is_same_transform(numpy.identity(4), numpy.identity(4))
1875 | True
1876 | >>> is_same_transform(numpy.identity(4), random_rotation_matrix())
1877 | False
1878 |
1879 | """
1880 | matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True)
1881 | matrix0 /= matrix0[3, 3]
1882 | matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True)
1883 | matrix1 /= matrix1[3, 3]
1884 | return numpy.allclose(matrix0, matrix1)
1885 |
1886 |
1887 | def is_same_quaternion(q0, q1):
1888 | """Return True if two quaternions are equal."""
1889 | q0 = numpy.array(q0)
1890 | q1 = numpy.array(q1)
1891 | return numpy.allclose(q0, q1) or numpy.allclose(q0, -q1)
1892 |
1893 |
1894 | def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'):
1895 | """Try import all public attributes from module into global namespace.
1896 |
1897 | Existing attributes with name clashes are renamed with prefix.
1898 | Attributes starting with underscore are ignored by default.
1899 |
1900 | Return True on successful import.
1901 |
1902 | """
1903 | import warnings
1904 | from importlib import import_module
1905 | try:
1906 | if not package:
1907 | module = import_module(name)
1908 | else:
1909 | module = import_module('.' + name, package=package)
1910 | except ImportError:
1911 | if warn:
1912 | warnings.warn("failed to import module %s" % name)
1913 | else:
1914 | for attr in dir(module):
1915 | if ignore and attr.startswith(ignore):
1916 | continue
1917 | if prefix:
1918 | if attr in globals():
1919 | globals()[prefix + attr] = globals()[attr]
1920 | elif warn:
1921 | warnings.warn("no Python implementation of " + attr)
1922 | globals()[attr] = getattr(module, attr)
1923 | return True
1924 |
1925 |
1926 | _import_module('_transformations')
1927 |
1928 | if __name__ == "__main__":
1929 | import doctest
1930 | import random # noqa: used in doctests
1931 |
1932 | numpy.set_printoptions(suppress=True, precision=5)
1933 | doctest.testmod()
1934 |
--------------------------------------------------------------------------------