├── 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 | ![avatar](https://github.com/xing-shuai/PaperDemoAndExampleTest/blob/master/resource/h36m_3d_visualization.gif) 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 | --------------------------------------------------------------------------------