├── profiles ├── solve_2 ├── solve_3 ├── solve_4 ├── solve_5 ├── solve_6 ├── solve_7 ├── solve_8 ├── solve_9 ├── solve_10 ├── solve_11 ├── solve_12 ├── solve_13 ├── solve_14 ├── solve_15 ├── updateTSDF_1 ├── updateTSDF_2 ├── updateTSDF_3 ├── updateTSDF_4 ├── updateTSDF_5 ├── updateTSDF_6 ├── updateTSDF_7 ├── updateTSDF_8 ├── updateTSDF_9 ├── updateTSDF_10 ├── updateTSDF_11 ├── updateTSDF_12 ├── updateTSDF_13 ├── updateTSDF_14 ├── updateTSDF_15 └── printProfile.py ├── .gitignore ├── core ├── models │ └── README.txt ├── gl │ ├── default.fs │ ├── default.vs │ ├── default.gs │ ├── glm.py │ └── glrender.py ├── __init__.py ├── colorutil.py ├── meshutil.py ├── net.py ├── sdf.py ├── util.py ├── fusion.py ├── fusion_dm.py └── transformation.py ├── meshes └── README ├── README.md ├── requirements.txt └── test.py /profiles/solve_2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_2 -------------------------------------------------------------------------------- /profiles/solve_3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_3 -------------------------------------------------------------------------------- /profiles/solve_4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_4 -------------------------------------------------------------------------------- /profiles/solve_5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_5 -------------------------------------------------------------------------------- /profiles/solve_6: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_6 -------------------------------------------------------------------------------- /profiles/solve_7: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_7 -------------------------------------------------------------------------------- /profiles/solve_8: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_8 -------------------------------------------------------------------------------- /profiles/solve_9: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_9 -------------------------------------------------------------------------------- /profiles/solve_10: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_10 -------------------------------------------------------------------------------- /profiles/solve_11: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_11 -------------------------------------------------------------------------------- /profiles/solve_12: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_12 -------------------------------------------------------------------------------- /profiles/solve_13: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_13 -------------------------------------------------------------------------------- /profiles/solve_14: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_14 -------------------------------------------------------------------------------- /profiles/solve_15: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/solve_15 -------------------------------------------------------------------------------- /profiles/updateTSDF_1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_1 -------------------------------------------------------------------------------- /profiles/updateTSDF_2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_2 -------------------------------------------------------------------------------- /profiles/updateTSDF_3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_3 -------------------------------------------------------------------------------- /profiles/updateTSDF_4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_4 -------------------------------------------------------------------------------- /profiles/updateTSDF_5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_5 -------------------------------------------------------------------------------- /profiles/updateTSDF_6: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_6 -------------------------------------------------------------------------------- /profiles/updateTSDF_7: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_7 -------------------------------------------------------------------------------- /profiles/updateTSDF_8: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_8 -------------------------------------------------------------------------------- /profiles/updateTSDF_9: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_9 -------------------------------------------------------------------------------- /profiles/updateTSDF_10: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_10 -------------------------------------------------------------------------------- /profiles/updateTSDF_11: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_11 -------------------------------------------------------------------------------- /profiles/updateTSDF_12: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_12 -------------------------------------------------------------------------------- /profiles/updateTSDF_13: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_13 -------------------------------------------------------------------------------- /profiles/updateTSDF_14: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_14 -------------------------------------------------------------------------------- /profiles/updateTSDF_15: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nintendops/DynamicFusion_Body/HEAD/profiles/updateTSDF_15 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | #* 3 | *.pyc 4 | __pycache__ 5 | ENV/ 6 | data/ 7 | data_backup/ 8 | core/models/* 9 | 10 | -------------------------------------------------------------------------------- /core/models/README.txt: -------------------------------------------------------------------------------- 1 | Download and unzip all files here 2 | https://drive.google.com/open?id=1-gWL9_r_hmHT1SGM_vJXHqVhS7Bl-MKg 3 | 4 | File structure should be: 5 | core/models/model.index 6 | core/models/model.meta 7 | core/models/model.model.data-00000-of-00001 -------------------------------------------------------------------------------- /core/gl/default.fs: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | flat in vec3 fColor[3]; 4 | in vec3 coord; 5 | out vec4 outColor; 6 | 7 | void main(){ 8 | int i = (coord.x > coord.y && coord.x > coord.z) ? 0 : ((coord.y > coord.z) ? 1 : 2); 9 | outColor = vec4(fColor[i], 1.0); 10 | } -------------------------------------------------------------------------------- /core/gl/default.vs: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(location = 0) in vec3 vertexPosition_modelspace; 4 | layout(location = 1) in vec3 vertexColor; 5 | 6 | uniform mat4 MVP; 7 | out vec3 vColor; 8 | 9 | void main(){ 10 | gl_Position = MVP * vec4(vertexPosition_modelspace, 1); 11 | vColor = vertexColor; 12 | } -------------------------------------------------------------------------------- /core/__init__.py: -------------------------------------------------------------------------------- 1 | # place all module imports here 2 | from .sdf import * 3 | from .fusion import Fusion 4 | from .fusion_dm import FusionDM 5 | import os 6 | 7 | # place all global variables here (or in the main script) 8 | DATA_PATH = os.path.join(os.path.dirname(os.path.dirname(os.path.realpath(__file__))), 'data/depth') 9 | 10 | -------------------------------------------------------------------------------- /meshes/README: -------------------------------------------------------------------------------- 1 | original: mesh extracted from the first mesh (canonical) 2 | high_reg_no_pruning_ICP: mesh fused with high regularization weight. Bad correspondences found by ICP were not rejected. 3 | low_reg_with_pruning_ICP: mesh fused with low regularization weight. Bad correspondences found by ICP were rejected. 4 | low_reg_with_pruning_CNN: mesh fused with low regularization weight. Bad correspondences found by CNN were rejected. 5 | 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | DynamicFusion Implementation, adapted to reconstruct body from multi-view images/ranged data 2 | 3 | To-do: 4 | 1. Model for volumetric warp field 5 | - sparse correspondences: computed elsewhere? 6 | - correspondences -> SE3 7 | - DQB interpolation 8 | 9 | 2. TSDF Fusion 10 | - backprojection: canonical (voxel center x_c) -> live frame (x_t) 11 | - live frame (x_t) -> Projective TSDF 12 | - need camera intrinsic 13 | - TSDF update (v', w') 14 | 15 | -------------------------------------------------------------------------------- /core/gl/default.gs: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | layout(triangles) in; 4 | layout(triangle_strip, max_vertices = 3) out; 5 | 6 | in vec3 vColor[]; 7 | flat out vec3 fColor[3]; 8 | out vec3 coord; 9 | 10 | void main() { 11 | for (int i = 0; i < 3; ++i) 12 | fColor[i] = vColor[i]; 13 | for (int i = 0; i < 3; ++i) { 14 | coord = vec3(0.0); 15 | coord[i] = 1.0; 16 | gl_Position = gl_in[i].gl_Position; 17 | EmitVertex(); 18 | } 19 | EndPrimitive(); 20 | } -------------------------------------------------------------------------------- /profiles/printProfile.py: -------------------------------------------------------------------------------- 1 | import pstats 2 | import sys 3 | 4 | if __name__ == "__main__": 5 | if len(sys.argv) < 2: 6 | print("Usage: python printProfile.py profilename [cumtime|internal|all]") 7 | exit() 8 | filename = sys.argv[1] 9 | p = pstats.Stats(filename) 10 | if len(sys.argv) == 3: 11 | flag = sys.argv[2] 12 | if flag == 'cumtime': 13 | p.sort_stats('cumulative').print_stats(15) 14 | elif flag == 'all': 15 | p.sort_stats('cumulative').print_stats() 16 | elif flag == 'internal': 17 | p.sort_stats('time').print_stats(10) 18 | else: 19 | print('Invalid flag') 20 | else: 21 | p.sort_stats('cumulative').print_stats(15) 22 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | absl-py==0.2.1 2 | astor==0.6.2 3 | bleach==1.5.0 4 | cycler==0.10.0 5 | decorator==4.3.0 6 | enum34==1.1.6 7 | gast==0.2.0 8 | grpcio==1.12.0 9 | ipython==5.6.0 10 | ipython-genutils==0.2.0 11 | kiwisolver==1.0.1 12 | llvmlite==0.23.0 13 | Markdown==2.6.11 14 | matplotlib==2.2.2 15 | mpmath==1.0.0 16 | networkx==2.1 17 | numba==0.38.0 18 | numpy==1.14.3 19 | opencv-contrib-python==3.4.0.12 20 | opencv-python==3.4.0.12 21 | pandas==0.22.0 22 | pathlib2==2.3.2 23 | pexpect==4.5.0 24 | pickleshare==0.7.4 25 | Pillow==5.1.0 26 | pkg-resources==0.0.0 27 | prompt-toolkit==1.0.15 28 | protobuf==3.5.2.post1 29 | ptyprocess==0.5.2 30 | PyOpenGL==3.1.0 31 | pyparsing==2.2.0 32 | python-dateutil==2.7.2 33 | pytz==2018.4 34 | PyWavelets==0.5.2 35 | scikit-image==0.13.1 36 | scipy==1.1.0 37 | simplegeneric==0.8.1 38 | six==1.11.0 39 | sympy==1.1.1 40 | tensorboard==1.8.0 41 | tensorflow==1.8.0 42 | termcolor==1.1.0 43 | traitlets==4.3.2 44 | wcwidth==0.1.7 45 | Werkzeug==0.14.1 46 | -------------------------------------------------------------------------------- /core/colorutil.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def idx2color(idx): 5 | r = idx // (256 * 256) % 256 6 | g = idx // 256 % 256 7 | b = idx % 256 8 | return np.array([r, g, b], dtype=np.uint8) 9 | 10 | 11 | def image_color2idx(color_img, rgb=False): 12 | color_img = color_img.astype(np.int32) 13 | idx = np.zeros([color_img.shape[0], color_img.shape[1]], np.int32) 14 | if rgb: 15 | idx[:, :] += color_img[:, :, 2] * 256 * 256 16 | idx[:, :] += color_img[:, :, 1] * 256 17 | idx[:, :] += color_img[:, :, 0] 18 | else: 19 | idx[:, :] += color_img[:, :, 0] * 256 * 256 20 | idx[:, :] += color_img[:, :, 1] * 256 21 | idx[:, :] += color_img[:, :, 2] 22 | return idx 23 | 24 | 25 | def image_int2color(int_img, rgb=False): 26 | color_img = np.zeros([int_img.shape[0], int_img.shape[1], 3], np.uint8) 27 | color_img[:, :, 0] = int_img // (256 * 256) % 256 28 | color_img[:, :, 1] = int_img // 256 % 256 29 | color_img[:, :, 2] = int_img % 256 30 | return color_img 31 | 32 | 33 | def distinct_colors(num_classes): 34 | colors = np.zeros([num_classes, 3], np.uint8) 35 | for i in range(num_classes): 36 | colors[i] = idx2color(i + 1) 37 | return colors 38 | -------------------------------------------------------------------------------- /core/meshutil.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from.gl import glm as glm 3 | 4 | 5 | def sqr_dist(src, dst): 6 | sqr_dists = np.multiply(np.dot(dst, src.T), -2) 7 | sqr_dists += np.sum(np.square(dst), 1, keepdims=True) 8 | sqr_dists += np.sum(np.square(src), 1) 9 | return sqr_dists 10 | 11 | 12 | def load_obj_mesh(mesh_path): 13 | vertex_data = [] 14 | face_data = [] 15 | for line in open(mesh_path, "r"): 16 | if line.startswith('#'): 17 | continue 18 | values = line.split() 19 | if not values: 20 | continue 21 | if values[0] == 'v': 22 | v = list(map(float, values[1:4])) 23 | vertex_data.append(v) 24 | elif values[0] == 'f': 25 | f = list(map(lambda x: int(x.split('/')[0]), values[1:4])) 26 | face_data.append(f) 27 | vertices = np.array(vertex_data) 28 | faces = np.array(face_data) 29 | return vertices, faces 30 | 31 | 32 | def load_mesh(mesh_path): 33 | if mesh_path.endswith('.obj'): 34 | vertices, faces = load_obj_mesh(mesh_path) 35 | if np.min(faces) == 1: 36 | faces -= 1 37 | vertices = vertices.astype(np.float32) 38 | faces = faces.astype(np.int32) 39 | return vertices, faces 40 | 41 | 42 | def regularize_mesh_old(vertices, model): 43 | tmp = np.ones([vertices.shape[0], 4], dtype=np.float32) 44 | tmp[:, :3] = vertices 45 | 46 | if model.startswith('SCAPE'): 47 | m = glm.identity() 48 | m = glm.rotate(m, glm.radians(90), glm.vec3(0, 0, 1)) 49 | m = glm.rotate(m, glm.radians(270), glm.vec3(1, 0, 0)) 50 | tmp = glm.transform(tmp, m) 51 | elif model.startswith('MIT'): 52 | m = glm.identity() 53 | m = glm.rotate(m, glm.radians(90), glm.vec3(0, 1, 0)) 54 | tmp = glm.transform(tmp, m) 55 | 56 | vertices[:, :] = tmp[:, :3] 57 | 58 | mean = np.mean(vertices, 0) 59 | vertices -= mean 60 | 61 | 62 | def regularize_mesh(vertices, flipyz): 63 | if flipyz: 64 | vertices[:, 1], vertices[:, 2] = vertices[:, 2], vertices[:, 1] 65 | 66 | scale = 1.8 / (np.max(vertices[:, 1]) - np.min(vertices[:, 1])) 67 | transform = -np.mean(vertices, 0) 68 | vertices = (vertices + transform) * scale 69 | return vertices 70 | 71 | 72 | def furthest_point_sample(vertices, faces, N, K): 73 | num_vertices = vertices.shape[0] 74 | center_indices = np.random.choice(num_vertices, N, replace=False) 75 | sqr_dists = 1e10 * np.ones(num_vertices) 76 | vertex_as = np.zeros(num_vertices, dtype=np.int32) 77 | for i in range(N): 78 | new_sqr_dists = np.sum(np.square(vertices - vertices[center_indices[i]]), 1) 79 | update_mask = new_sqr_dists < sqr_dists 80 | sqr_dists[update_mask] = new_sqr_dists[update_mask] 81 | vertex_as[update_mask] = i 82 | next_center = np.argmax(sqr_dists) 83 | if K - 1 <= i < N - 1: 84 | center_indices[i + 1] = next_center 85 | 86 | centers = vertices[center_indices] 87 | face_centers = np.mean(vertices[faces], 1) 88 | sqr_dists = sqr_dist(centers, face_centers) 89 | face_as = np.argmin(sqr_dists, 1) 90 | return center_indices, vertex_as, face_as 91 | -------------------------------------------------------------------------------- /core/gl/glm.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | def vec3(x, y, z): 6 | return np.array([x, y, z], dtype=np.float32) 7 | 8 | 9 | def radians(v): 10 | return np.radians(v) 11 | 12 | 13 | def identity(): 14 | return np.identity(4, dtype=np.float32) 15 | 16 | 17 | def empty(): 18 | return np.zeros([4, 4], dtype=np.float32) 19 | 20 | 21 | def magnitude(v): 22 | return np.linalg.norm(v) 23 | 24 | 25 | def normalize(v): 26 | m = magnitude(v) 27 | return v if m == 0 else v / m 28 | 29 | 30 | def dot(u, v): 31 | return np.sum(u * v) 32 | 33 | 34 | def cross(u, v): 35 | res = vec3(0, 0, 0) 36 | res[0] = u[1] * v[2] - u[2] * v[1] 37 | res[1] = u[2] * v[0] - u[0] * v[2] 38 | res[2] = u[0] * v[1] - u[1] * v[0] 39 | return res 40 | 41 | 42 | # below functions can be optimized 43 | 44 | def translate(m, v): 45 | res = np.copy(m) 46 | res[:, 3] = m[:, 0] * v[0] + m[:, 1] * v[1] + m[:, 2] * v[2] + m[:, 3] 47 | return res 48 | 49 | 50 | def rotate(m, angle, v): 51 | a = angle 52 | c = np.cos(a) 53 | s = np.sin(a) 54 | 55 | axis = normalize(v) 56 | temp = (1 - c) * axis 57 | 58 | rot = empty() 59 | rot[0][0] = c + temp[0] * axis[0] 60 | rot[0][1] = temp[0] * axis[1] + s * axis[2] 61 | rot[0][2] = temp[0] * axis[2] - s * axis[1] 62 | 63 | rot[1][0] = temp[1] * axis[0] - s * axis[2] 64 | rot[1][1] = c + temp[1] * axis[1] 65 | rot[1][2] = temp[1] * axis[2] + s * axis[0] 66 | 67 | rot[2][0] = temp[2] * axis[0] + s * axis[1] 68 | rot[2][1] = temp[2] * axis[1] - s * axis[0] 69 | rot[2][2] = c + temp[2] * axis[2] 70 | 71 | res = empty() 72 | res[:, 0] = m[:, 0] * rot[0][0] + m[:, 1] * rot[0][1] + m[:, 2] * rot[0][2] 73 | res[:, 1] = m[:, 0] * rot[1][0] + m[:, 1] * rot[1][1] + m[:, 2] * rot[1][2] 74 | res[:, 2] = m[:, 0] * rot[2][0] + m[:, 1] * rot[2][1] + m[:, 2] * rot[2][2] 75 | res[:, 3] = m[:, 3] 76 | return res 77 | 78 | 79 | def perspective(fovy, aspect, zNear, zFar): 80 | tanHalfFovy = np.tan(fovy / 2) 81 | 82 | res = empty() 83 | res[0][0] = 1 / (aspect * tanHalfFovy) 84 | res[1][1] = 1 / (tanHalfFovy) 85 | res[2][3] = -1 86 | res[2][2] = - (zFar + zNear) / (zFar - zNear) 87 | res[3][2] = -(2 * zFar * zNear) / (zFar - zNear) 88 | 89 | return res.T 90 | 91 | 92 | def ortho(left, right, bottom, top, zNear, zFar): 93 | # res = np.ones([4, 4], dtype=np.float32) 94 | res = identity() 95 | res[0][0] = 2 / (right - left) 96 | res[1][1] = 2 / (top - bottom) 97 | res[2][2] = - 2 / (zFar - zNear) 98 | res[3][0] = - (right + left) / (right - left) 99 | res[3][1] = - (top + bottom) / (top - bottom) 100 | res[3][2] = - (zFar + zNear) / (zFar - zNear) 101 | return res.T 102 | 103 | 104 | def lookat(eye, center, up): 105 | f = normalize(center - eye) 106 | s = normalize(cross(f, up)) 107 | u = cross(s, f) 108 | 109 | res = identity() 110 | res[0][0] = s[0] 111 | res[1][0] = s[1] 112 | res[2][0] = s[2] 113 | res[0][1] = u[0] 114 | res[1][1] = u[1] 115 | res[2][1] = u[2] 116 | res[0][2] = -f[0] 117 | res[1][2] = -f[1] 118 | res[2][2] = -f[2] 119 | res[3][0] = -dot(s, eye) 120 | res[3][1] = -dot(u, eye) 121 | res[3][2] = -dot(f, eye) 122 | return res.T 123 | 124 | 125 | def transform(d, m): 126 | return np.dot(m, d.T).T 127 | -------------------------------------------------------------------------------- /core/net.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import tensorflow.contrib.slim as slim 3 | import numpy as np 4 | 5 | 6 | class DHBC: 7 | def __init__(self): 8 | self.feature = None 9 | self.feat_vars = None 10 | self.preds = {} 11 | self.clas_vars = {} 12 | self.losses = {} 13 | self.train_ops = {} 14 | 15 | def forward(self, input): 16 | self.depth = input 17 | conv = self._conv 18 | upconv = self._upconv 19 | maxpool = self._maxpool 20 | with tf.variable_scope('feature'): 21 | with tf.variable_scope('encoder'): 22 | conv1 = conv(self.depth, 96, 11, 4) # H/4 23 | pool1 = maxpool(conv1, 3) # H/8 24 | conv2 = conv(pool1, 256, 5, 1) # H/8 25 | pool2 = maxpool(conv2, 3) # H/16 26 | conv3 = conv(pool2, 384, 3, 1) # H/16 27 | conv4 = conv(conv3, 384, 3, 1) # H/16 28 | conv5 = conv(conv4, 256, 3, 1) # H/16 29 | pool5 = maxpool(conv5, 3) # H/32 30 | conv6 = conv(pool5, 4096, 1, 1) # H/32 31 | conv7 = conv(conv6, 4096, 1, 1) # H/32 32 | 33 | with tf.variable_scope('skips'): 34 | skip1 = conv1 35 | skip2 = conv2 36 | skip3 = conv5 37 | 38 | with tf.variable_scope('decoder'): 39 | upconv5 = upconv(conv7, 256, 3, 2) # H/16 40 | concat5 = tf.concat([upconv5, skip3], 3) 41 | iconv5 = conv(concat5, 256, 3, 1) 42 | 43 | upconv4 = upconv(iconv5, 256, 3, 2) # H/8 44 | concat4 = tf.concat([upconv4, skip2], 3) 45 | iconv4 = conv(concat4, 256, 3, 1) 46 | 47 | upconv3 = upconv(iconv4, 96, 3, 2) # H/4 48 | concat3 = tf.concat([upconv3, skip1], 3) 49 | iconv3 = conv(concat3, 96, 3, 1) 50 | 51 | upconv2 = upconv(iconv3, 48, 3, 2) # H/2 52 | upconv1 = upconv(upconv2, 16, 3, 2) # H/1 53 | self.feature = upconv1 54 | 55 | self.feat_vars = slim.get_model_variables() 56 | return self.feature 57 | 58 | def classify(model_range, seg_range, feature_lr, classifier_lr): 59 | feat_opt = tf.train.AdamOptimizer(feature_lr) 60 | clas_opt = tf.train.AdamOptimizer(classifier_lr) 61 | for model in model_range: 62 | for seg in seg_range: 63 | with tf.variable_scope('classifier-{}-{}'.format(model, seg)): 64 | self.preds[(model, seg)] = slim.conv2d(self.feature, 500, [1, 1]) 65 | self.clas_vars[(model, seg)] = slim.get_model_variables()[-2:] 66 | 67 | with tf.variable_scope('losses-{}-{}'.format(model, seg)): 68 | self.losses[(model, seg)] = self.loss(self.labels, self.preds[(model, seg)]) 69 | grad = tf.gradients(self.losses[(model, seg)], self.feat_vars + self.clas_vars[(model, seg)]) 70 | train_op_feat = feat_opt.apply_gradients(zip(grad[:-2], self.feat_vars)) 71 | train_op_clas = clas_opt.apply_gradients(zip(grad[-2:], self.clas_vars[(model, seg)])) 72 | self.train_ops[(model, seg)] = tf.group(train_op_feat, train_op_clas) 73 | return self.losses, self.train_ops 74 | 75 | def _loss(self, labels, logits): 76 | float_labels = tf.cast(labels, tf.float32) 77 | 78 | epsilon = tf.constant(value=1e-4) 79 | softmax = tf.nn.softmax(logits) + epsilon 80 | cross_entropy = -tf.reduce_sum(float_labels * tf.log(softmax), reduction_indices=[-1]) 81 | cross_entropy_mean = tf.reduce_mean(cross_entropy) 82 | 83 | total_pixels = tf.constant(value=conf.width * conf.height, dtype=tf.float32) 84 | valid_pixels = tf.reduce_sum(float_labels) 85 | loss = tf.divide(tf.multiply(cross_entropy_mean, total_pixels), valid_pixels) 86 | 87 | return loss 88 | 89 | def _conv_block(self, x, num_out_layers, kernel_size): 90 | conv1 = self._conv(x, num_out_layers, kernel_size, 1) 91 | conv2 = self._conv(conv1, num_out_layers, kernel_size, 2) 92 | return conv2 93 | 94 | def _conv(self, x, num_out_layers, kernel_size, stride, activation_fn=tf.nn.elu): 95 | p = np.floor((kernel_size - 1) / 2).astype(np.int32) 96 | p_x = tf.pad(x, [[0, 0], [p, p], [p, p], [0, 0]]) 97 | return slim.conv2d(p_x, num_out_layers, kernel_size, stride, 'VALID', activation_fn=activation_fn) 98 | 99 | def _deconv(self, x, num_out_layers, kernel_size, scale): 100 | p_x = tf.pad(x, [[0, 0], [1, 1], [1, 1], [0, 0]]) 101 | conv = slim.conv2d_transpose(p_x, num_out_layers, kernel_size, scale, 'SAME') 102 | return conv[:, 3:-1, 3:-1, :] 103 | 104 | def _upconv(self, x, num_out_layers, kernel_size, scale): 105 | upsample = self._upsample_nn(x, scale) 106 | conv = self._conv(upsample, num_out_layers, kernel_size, 1) 107 | return conv 108 | 109 | def _upsample_nn(self, x, ratio): 110 | s = tf.shape(x) 111 | h = s[1] 112 | w = s[2] 113 | return tf.image.resize_nearest_neighbor(x, [h * ratio, w * ratio]) 114 | 115 | def _maxpool(self, x, kernel_size): 116 | p = np.floor((kernel_size - 1) / 2).astype(np.int32) 117 | p_x = tf.pad(x, [[0, 0], [p, p], [p, p], [0, 0]]) 118 | return slim.max_pool2d(p_x, kernel_size) 119 | -------------------------------------------------------------------------------- /core/gl/glrender.py: -------------------------------------------------------------------------------- 1 | from OpenGL.GLUT import * 2 | from OpenGL.GLU import * 3 | from OpenGL.GL import * 4 | import numpy as np 5 | 6 | 7 | def LoadProgram(shaderPathList): 8 | shaderTypeMapping = { 9 | 'vs': GL_VERTEX_SHADER, 10 | 'gs': GL_GEOMETRY_SHADER, 11 | 'fs': GL_FRAGMENT_SHADER 12 | } 13 | shaderTypeList = [shaderTypeMapping[shaderType] for shaderPath in shaderPathList for shaderType in shaderTypeMapping 14 | if shaderPath.endswith(shaderType)] 15 | shaders = [] 16 | for i in range(len(shaderPathList)): 17 | shaderPath = shaderPathList[i] 18 | shaderType = shaderTypeList[i] 19 | 20 | with open(shaderPath, 'r') as f: 21 | shaderData = f.read() 22 | shader = glCreateShader(shaderType) 23 | glShaderSource(shader, shaderData) 24 | glCompileShader(shader) 25 | shaders.append(shader) 26 | 27 | program = glCreateProgram() 28 | for shader in shaders: 29 | glAttachShader(program, shader) 30 | glLinkProgram(program) 31 | for shader in shaders: 32 | glDetachShader(program, shader) 33 | glDeleteShader(shader) 34 | return program 35 | 36 | 37 | class GLRenderer(object): 38 | def __init__(self, name, size, pos, toTexture=False, shaderPathList=None): 39 | self.width, self.height = size 40 | self.toTexture = toTexture 41 | 42 | glutInit() 43 | displayMode = GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH | GLUT_STENCIL | GLUT_BORDERLESS | GLUT_CAPTIONLESS 44 | glutInitDisplayMode(displayMode) 45 | glutInitWindowPosition(pos[0], pos[1]) 46 | glutInitWindowSize(self.width, self.height) 47 | self.window = glutCreateWindow(name) 48 | # glutFullScreen() 49 | glEnable(GL_CULL_FACE) 50 | glEnable(GL_DEPTH_TEST) 51 | glDepthFunc(GL_LESS) 52 | 53 | self.vertexArr = glGenVertexArrays(1) 54 | glBindVertexArray(self.vertexArr) 55 | self.vertexBuf = glGenBuffers(1) 56 | self.colorBuf = glGenBuffers(1) 57 | glClearColor(0.0, 0.0, 0.0, 0.0) 58 | 59 | self.toTexture = toTexture 60 | if toTexture: 61 | self.texColor = glGenTextures(1); 62 | glBindTexture(GL_TEXTURE_2D, self.texColor) 63 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE) 64 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE) 65 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST) 66 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST) 67 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, self.width, self.height, 0, GL_RGB, GL_UNSIGNED_BYTE, None) 68 | 69 | self.texDepth = glGenTextures(1) 70 | glBindTexture(GL_TEXTURE_2D, self.texDepth) 71 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT) 72 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT) 73 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST) 74 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST) 75 | glTexParameteri(GL_TEXTURE_2D, GL_DEPTH_TEXTURE_MODE, GL_INTENSITY) 76 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_R_TO_TEXTURE) 77 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL) 78 | glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, self.width, self.height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, 79 | None) 80 | 81 | self.frameBuf = glGenFramebuffers(1) 82 | glBindFramebuffer(GL_FRAMEBUFFER, self.frameBuf) 83 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, self.texColor, 0) 84 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, self.texDepth, 0) 85 | 86 | if not shaderPathList: 87 | dir_path = os.path.dirname(os.path.abspath(__file__)) 88 | shaderPathList = [os.path.join(dir_path, sh) for sh in ['default.vs', 'default.gs', 'default.fs']] 89 | # shaderPathList = [os.path.join('glhelper', sh) for sh in ['default.vs']] 90 | self.program = LoadProgram(shaderPathList) 91 | self.mvpMatrix = glGetUniformLocation(self.program, 'MVP') 92 | 93 | def draw(self, vertices, colors, mvp): 94 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) 95 | glUseProgram(self.program) 96 | glUniformMatrix4fv(self.mvpMatrix, 1, GL_FALSE, mvp) 97 | 98 | glEnableVertexAttribArray(0) 99 | glBindBuffer(GL_ARRAY_BUFFER, self.vertexBuf) 100 | glBufferData(GL_ARRAY_BUFFER, vertices, GL_STATIC_DRAW); 101 | glVertexAttribPointer( 102 | 0, # attribute 103 | vertices.shape[1], # size 104 | GL_FLOAT, # type 105 | GL_FALSE, # normalized? 106 | 0, # stride 107 | None # array buffer offset 108 | ); 109 | 110 | glEnableVertexAttribArray(1) 111 | glBindBuffer(GL_ARRAY_BUFFER, self.colorBuf) 112 | glBufferData(GL_ARRAY_BUFFER, colors, GL_STATIC_DRAW); 113 | glVertexAttribPointer( 114 | 1, # attribute 115 | colors.shape[1], # size 116 | GL_FLOAT, # type 117 | GL_FALSE, # normalized? 118 | 0, # stride 119 | None # array buffer offset 120 | ); 121 | 122 | glDrawArrays(GL_TRIANGLES, 0, vertices.shape[0]) 123 | glDisableVertexAttribArray(0) 124 | glDisableVertexAttribArray(1) 125 | glUseProgram(0) 126 | glutSwapBuffers() 127 | 128 | rgb = glReadPixels(0, 0, self.width, self.height, GL_RGB, GL_UNSIGNED_BYTE, outputType=None) 129 | z = glReadPixels(0, 0, self.width, self.height, GL_DEPTH_COMPONENT, GL_FLOAT, outputType=None) 130 | rgb = np.flip(np.flip(rgb, 0), 1) 131 | z = np.flip(np.flip(z, 0), 1) 132 | return rgb, z 133 | -------------------------------------------------------------------------------- /core/sdf.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from .gl import glm as glm 4 | from .gl.glrender import GLRenderer 5 | from .meshutil import regularize_mesh, load_mesh 6 | from .colorutil import distinct_colors, image_color2idx 7 | from .net import DHBC 8 | import tensorflow as tf 9 | 10 | ''' 11 | *** Signed distance field file is binary. Format: 12 | - resolutionX,resolutionY,resolutionZ (three signed 4-byte integers (all equal), 12 bytes total) 13 | - bminx,bminy,bminz (coordinates of the lower-left-front corner of the bounding box: (three double precision 8-byte real numbers , 24 bytes total) 14 | - bmaxx,bmaxy,bmaxz (coordinates of the upper-right-back corner of the bounding box: (three double precision 8-byte real numbers , 24 bytes total) 15 | - distance data (in single precision; data alignment: 16 | [0,0,0],...,[resolutionX,0,0], 17 | [0,1,0],...,[resolutionX,resolutionY,0], 18 | [0,0,1],...,[resolutionX,resolutionY,resolutionZ]; 19 | total num bytes: sizeof(float)*(resolutionX+1)*(resolutionY+1)*(resolutionZ+1)) 20 | - closest point for each grid vertex(3 coordinates in single precision) 21 | ''' 22 | 23 | 24 | def load_sdf(file_path, read_closest_points=False, verbose=False): 25 | ''' 26 | 27 | :param file_path: file path 28 | :param read_closest_points: whether to read closest points for each grid vertex 29 | :param verbose: verbose flag 30 | :return: 31 | b_min: coordinates of the lower-left-front corner of the bounding box 32 | b_max: coordinates of the upper-right-back corner of the bounding box 33 | volume: distance data in shape (resolutionX+1)*(resolutionY+1)*(resolutionZ+1) 34 | closest_points: closest points in shape (resolutionX+1)*(resolutionY+1)*(resolutionZ+1) 35 | ''' 36 | with open(file_path, 'rb') as fp: 37 | 38 | res_x = int(np.fromfile(fp, dtype=np.int32, 39 | count=1)) # note: the dimension of volume is (1+res_x) x (1+res_y) x (1+res_z) 40 | res_x = - res_x 41 | res_y = -int(np.fromfile(fp, dtype=np.int32, count=1)) 42 | res_z = int(np.fromfile(fp, dtype=np.int32, count=1)) 43 | if verbose: print("resolution: %d %d %d" % (res_x, res_y, res_z)) 44 | 45 | b_min = np.zeros(3, dtype=np.float64) 46 | b_min[0] = np.fromfile(fp, dtype=np.float64, count=1) 47 | b_min[1] = np.fromfile(fp, dtype=np.float64, count=1) 48 | b_min[2] = np.fromfile(fp, dtype=np.float64, count=1) 49 | if verbose: print("b_min: %f %f %f" % (b_min[0], b_min[1], b_min[2])) 50 | 51 | b_max = np.zeros(3, dtype=np.float64) 52 | b_max[0] = np.fromfile(fp, dtype=np.float64, count=1) 53 | b_max[1] = np.fromfile(fp, dtype=np.float64, count=1) 54 | b_max[2] = np.fromfile(fp, dtype=np.float64, count=1) 55 | if verbose: print("b_max: %f %f %f" % (b_max[0], b_max[1], b_max[2])) 56 | 57 | grid_num = (1 + res_x) * (1 + res_y) * (1 + res_z) 58 | volume = np.fromfile(fp, dtype=np.float32, count=grid_num) 59 | volume = volume.reshape(((1 + res_z), (1 + res_y), (1 + res_x))) 60 | volume = np.swapaxes(volume, 0, 2) 61 | if verbose: print("loaded volume from %s" % file_path) 62 | 63 | closest_points = None 64 | if read_closest_points: 65 | closest_points = np.fromfile(fp, dtype=np.float32, count=grid_num * 3) 66 | closest_points = closest_points.reshape(((1 + res_z), (1 + res_y), (1 + res_x), 3)) 67 | closest_points = np.swapaxes(closest_points, 0, 2) 68 | 69 | return b_min, b_max, volume, closest_points 70 | 71 | 72 | ############################## 73 | # The following comes all CNN code 74 | 75 | def cnnInitialize(): 76 | # Globally initialize a CNN sesseion 77 | print('Initialize network...') 78 | tf.Graph().as_default() 79 | dhbc = DHBC() 80 | input = tf.placeholder(tf.float32, [1, None, None, 1]) 81 | feature = dhbc.forward(input) 82 | 83 | path = os.path.dirname(os.path.abspath(__file__)) 84 | print(path) 85 | 86 | # Checkpoint 87 | checkpoint = path + '/models/model' 88 | print('Load checkpoit from {}...'.format(checkpoint)) 89 | sess = tf.Session() 90 | sess.run(tf.global_variables_initializer()) 91 | saver = tf.train.Saver(dhbc.feat_vars) 92 | saver.restore(sess, checkpoint) 93 | return input, feature, sess 94 | 95 | def compute_correspondence(input, feature, sess, vertices, faces, znear=1.0, zfar=3.5, max_swi=70, width=512, height=512, flipyz=False): 96 | ''' 97 | Compute a correspondence vector for mesh (vertices, faces) 98 | :param vertices: mesh vertices 99 | :param faces: mesh faces 100 | :param znear: 101 | :param zfar: 102 | :param max_swi: 103 | :param width: 104 | :param height: 105 | :param flipyz: 106 | :return: For each vertex a 16-digit correspondence vector. [N, 16] 107 | ''' 108 | b = zfar * znear / (znear - zfar) 109 | a = -b / znear 110 | 111 | renderer = GLRenderer(b'generate_mesh_feature', (width, height), (0, 0), toTexture=True) 112 | proj = glm.perspective(glm.radians(70), 1.0, znear, zfar) 113 | 114 | vertices = regularize_mesh(vertices, flipyz) 115 | 116 | faces = faces.reshape([faces.shape[0] * 3]) 117 | vertex_buffer = vertices[faces] 118 | vertex_color = distinct_colors(vertices.shape[0]) 119 | vertex_color_buffer = (vertex_color[faces] / 255.0).astype(np.float32) 120 | 121 | cnt = np.zeros([vertices.shape[0]], dtype=np.int32) 122 | feat = np.zeros([vertices.shape[0], 16], dtype=np.float32) 123 | 124 | swi = 35 125 | dis = 200 126 | for rot in range(0, 360, 15): 127 | mod = glm.identity() 128 | mod = glm.rotate(mod, glm.radians(swi - max_swi / 2), glm.vec3(0, 1, 0)) 129 | mod = glm.translate(mod, glm.vec3(0, 0, -dis / 100.0)) 130 | mod = glm.rotate(mod, glm.radians(rot), glm.vec3(0, 1, 0)) 131 | mvp = proj.dot(mod) 132 | 133 | rgb, z = renderer.draw(vertex_buffer, vertex_color_buffer, mvp.T) 134 | 135 | depth = ((zfar - b / (z - a)) / (zfar - znear) * 255).astype(np.uint8) 136 | features = sess.run(feature, feed_dict={input: depth.reshape([1, 512, 512, 1])}).reshape([512, 512, 16]) 137 | 138 | vertex = image_color2idx(rgb) 139 | 140 | mask = vertex > 0 141 | vertex = vertex[mask] 142 | features = features[mask] 143 | for i in range(vertex.shape[0]): 144 | cnt[vertex[i] - 1] += 1 145 | feat[vertex[i] - 1] += features[i] 146 | 147 | for i in range(vertices.shape[0]): 148 | if cnt[i] > 0: 149 | feat[i] /= cnt[i] 150 | return feat 151 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import sys 3 | import os 4 | import cProfile 5 | from numpy import linalg as la 6 | from skimage import measure 7 | from skimage.draw import ellipse, ellipsoid 8 | from scipy.spatial import KDTree 9 | from core import * 10 | from core.util import * 11 | from core.fusion_dm import FusionDM_GPU 12 | from core.transformation import random_rotation_matrix 13 | import matplotlib.pyplot as plt 14 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 15 | 16 | TEST_FUSION_DM = True 17 | TEST_FUSION_DM_GPU = True 18 | TEST_FUSION = False 19 | TEST_FUSION_DUMMY = False 20 | TEST_UTIL = False 21 | TEST_CUSTOM = False 22 | 23 | 24 | def visualize(tsdf): 25 | fig = plt.figure(figsize=(10, 10)) 26 | ax = fig.add_subplot(111, projection='3d') 27 | verts, faces, ns, vs = measure.marching_cubes_lewiner(tsdf, setp_size=1, allow_degenerate=False) 28 | mesh = Poly3DCollection(verts[faces]) 29 | mesh.set_edgecolor('k') 30 | ax.add_collection3d(mesh) 31 | ax.set_xlabel("x-axis: a = 6 per ellipsoid") 32 | ax.set_ylabel("y-axis: b = 10") 33 | ax.set_zlabel("z-axis: c = 16") 34 | ax.set_xlim(0, 24) # a = 6 (times two for 2nd ellipsoid) 35 | ax.set_ylim(0, 20) # b = 10 36 | ax.set_zlim(0, 32) # c = 16 37 | plt.tight_layout() 38 | plt.show() 39 | 40 | 41 | def readObj(filename): 42 | objFile = open(filename, 'r') 43 | vertexList = [] 44 | 45 | for line in objFile: 46 | split = line.split() 47 | if not len(split): 48 | continue 49 | if split[0] == 'v': 50 | vertexList.append(split[1:]) 51 | elif split[0] == 'vt': 52 | pass 53 | elif split[0] == 'f': 54 | pass 55 | elif split[0] == 'vn': 56 | pass 57 | 58 | return np.array(vertexList, dtype='float') 59 | 60 | 61 | if __name__ == "__main__": 62 | 63 | # Generate a level set about zero of two identical ellipsoids in 3D 64 | ellip_base = ellipsoid(6, 10, 16, levelset=True) 65 | e2 = ellipsoid(6, 10, 16, levelset=True) 66 | volume = ellip_base[:-1, ...] 67 | volume2 = e2[:-1, ...] 68 | 69 | output_mesh_name = 'mesh.obj' 70 | if len(sys.argv) >= 2: 71 | output_mesh_name = sys.argv[1] 72 | 73 | if TEST_FUSION_DUMMY: 74 | fus = Fusion(volume, volume.max(), marching_cubes_step_size=0.5, subsample_rate=2, verbose=True) 75 | print("Solving for a test iteration") 76 | fus.setupCorrespondences(volume2, method='clpts') 77 | fus.solve(method='clpts', tukey_data_weight=1, regularization_weight=10) 78 | print("Updating TSDF...") 79 | fus.updateTSDF() 80 | print("Updating deformation graph...") 81 | fus.update_graph() 82 | 83 | # Display resulting triangular mesh using Matplotlib. This can also be done 84 | # with mayavi (see skimage.measure.marching_cubes_lewiner docstring). 85 | fig = plt.figure(figsize=(10, 10)) 86 | ax = fig.add_subplot(111, projection='3d') 87 | # Fancy indexing: `verts[faces]` to generate a collection of triangles 88 | verts, faces, ns, vs = measure.marching_cubes_lewiner(fus._tsdf, 89 | step_size=1, 90 | allow_degenerate=False) 91 | 92 | mesh = Poly3DCollection(verts[faces]) 93 | mesh.set_edgecolor('k') 94 | ax.add_collection3d(mesh) 95 | ax.set_xlabel("x-axis: a = 6 per ellipsoid") 96 | ax.set_ylabel("y-axis: b = 10") 97 | ax.set_zlabel("z-axis: c = 16") 98 | ax.set_xlim(0, 24) # a = 6 (times two for 2nd ellipsoid) 99 | ax.set_ylim(0, 20) # b = 10 100 | ax.set_zlim(0, 32) # c = 16 101 | plt.tight_layout() 102 | plt.show() 103 | 104 | if TEST_FUSION: 105 | sdf_filepath = os.path.join(DATA_PATH, '0000.64.dist') 106 | b_min, b_max, volume, closest_points = load_sdf(sdf_filepath, verbose=True) 107 | print(volume.max()) 108 | if TEST_FUSION: 109 | # Generate a level set about zero of two identical ellipsoids in 3D 110 | fus = Fusion(volume, volume.max(), subsample_rate=1.5, knn=3, marching_cubes_step_size=2, verbose=True, 111 | use_cnn=False) 112 | fus.write_canonical_mesh(DATA_PATH, 'original.obj') 113 | f_iter = 1 114 | datas = os.listdir(DATA_PATH) 115 | datas.sort() 116 | for tfile in datas: 117 | if f_iter > 10: 118 | break 119 | if tfile.endswith('64.dist') and not tfile.endswith('0000.64.dist'): 120 | try: 121 | b_min1, b_max1, volume, closest_points1 = load_sdf(os.path.join(DATA_PATH, tfile), verbose=True) 122 | print("Processing iteration:", f_iter) 123 | print("New shape of volume: (%d, %d, %d)" % volume.shape) 124 | print("Setting up correspondences...") 125 | fus.setupCorrespondences(volume, method='clpts') 126 | cProfile.run('fus.solve(regularization_weight=0.5, method = "clpts")', 127 | 'profiles/solve_' + str(f_iter)) 128 | print("Updating TSDF...") 129 | cProfile.run('fus.updateTSDF()', 'profiles/updateTSDF_' + str(f_iter)) 130 | print("Updating deformation graph...") 131 | fus.update_graph() 132 | f_iter += 1 133 | except ValueError as e: 134 | print(str(e)) 135 | break 136 | except KeyboardInterrupt: 137 | break 138 | fus.write_canonical_mesh(DATA_PATH, output_mesh_name) 139 | 140 | if TEST_FUSION_DM: 141 | K = np.array([2000, 0, 800, 0, 2000, 600, 0, 0, 1], dtype='float').reshape(3, 3) 142 | Kinv = la.inv(K) 143 | datas = os.listdir(DATA_PATH) 144 | datas = sorted(datas) 145 | depths = [] 146 | lws = [] 147 | for fname in datas: 148 | path = os.path.join(DATA_PATH, fname) 149 | if fname.endswith('.npy'): 150 | print(fname) 151 | depths.append(np.load(path)) 152 | elif fname.startswith('proj') and fname.endswith('.txt'): 153 | print(fname) 154 | P = read_proj_matrix(path) 155 | lws.append(np.matmul(Kinv, P)) 156 | 157 | print("loaded (%d, %d) of depths and matrices" % (len(depths), len(lws))) 158 | if TEST_FUSION_DM_GPU: 159 | fus = FusionDM_GPU(0.2, K, tsdf_res=256, verbose=True) 160 | else: 161 | fus = FusionDM(0.2, K, tsdf_res=256, verbose=True) 162 | 163 | fus.compute_live_tsdf(depths, lws, useICP=False, outputMesh=True) 164 | 165 | if TEST_CUSTOM: 166 | K = np.array([2000, 0, 800, 0, 2000, 600, 0, 0, 1], dtype='float').reshape(3, 3) 167 | Kinv = la.inv(K) 168 | datas = os.listdir(DATA_PATH) 169 | datas = sorted(datas) 170 | depths = [] 171 | lws = [] 172 | for fname in datas: 173 | path = os.path.join(DATA_PATH, fname) 174 | if fname.endswith('.npy'): 175 | print(fname) 176 | depths.append(np.load(path)) 177 | elif fname.endswith('.txt') and fname.startswith('proj'): 178 | print(fname) 179 | P = read_proj_matrix(path) 180 | lws.append(np.matmul(Kinv, P)) 181 | gtverts = readObj(os.path.join(DATA_PATH, 'Jamie.obj')) 182 | kdt = KDTree(gtverts) 183 | 184 | for idx in range(2): 185 | fpath = open(os.path.join(DATA_PATH, 'transformed_pts' + str(idx) + '.txt'), 'w') 186 | score = 0 187 | dm = depths[idx] 188 | lw = lws[idx] 189 | lw_inv = inverse_rigid_matrix(lw) 190 | dmx, dmy = dm.shape 191 | for dx in range(dmx): 192 | for dy in range(dmy): 193 | uv = np.array([dy, dx, 1], dtype='float') 194 | d = -1 * dm[dx, dy] 195 | if d > 0: 196 | pos_local = np.matmul(Kinv, d * uv) 197 | pos_global = np.matmul(lw_inv, np.append(pos_local, 1)) 198 | # dist, nidx = kdt.query(pos_global) 199 | fpath.write('%f %f %f\n' % (pos_global[0], pos_global[1], pos_global[2])) 200 | fpath.close() 201 | 202 | if TEST_UTIL: 203 | # Testing DQ functions 204 | print('Testing DQ functions') 205 | R = random_rotation_matrix()[np.ix_([0, 1, 2], [0, 1, 2])] 206 | t = np.array([0.1, 0.4, 0.2]) 207 | M = compose_se3(R, t) 208 | print("Input matrix") 209 | print(M) 210 | print('converted dq') 211 | q = SE3TDQ(M) 212 | print(q) 213 | print('converted back to matrix') 214 | print(DQTSE3(q)) 215 | # Testing interpolation 216 | print('Testing tsdf trilinear interpolation') 217 | for i in range(3): 218 | pos1 = res_x / 2 * np.random.rand(3) 219 | pos2 = res_y / 2 * np.random.rand(3) 220 | pos3 = (10, 10, 10) 221 | posb1 = -1 * np.random.rand(3) 222 | posb2 = np.array(volume.shape) + 1 223 | print('below should not be None') 224 | print(interpolate_tsdf(pos1, volume)) 225 | print(interpolate_tsdf(pos2, volume)) 226 | print('ground truth %f' % volume[pos3]) 227 | print(interpolate_tsdf(pos3, volume)) 228 | print('below should be None') 229 | print(interpolate_tsdf(posb1, volume)) 230 | print(interpolate_tsdf(posb2, volume)) 231 | -------------------------------------------------------------------------------- /core/util.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from numpy import linalg as la 4 | 5 | ''' 6 | This script contains the utility functions for certain mathematical computations 7 | ''' 8 | 9 | # epsilon for testing whether a number is close to zero 10 | _EPS = np.finfo(float).eps * 4.0 11 | 12 | 13 | # Compose a 4x4 SE3 matrix 14 | def compose_se3(R,t): 15 | M = np.identity(4) 16 | M[0] = np.append(R[0],t[0]) 17 | M[1] = np.append(R[1],t[1]) 18 | M[2] = np.append(R[2],t[2]) 19 | return M 20 | 21 | # Decompose M into R and t 22 | def decompose_se3(M): 23 | return M[np.ix_([0,1,2],[0,1,2])], M[np.ix_([0,1,2],[3])] 24 | 25 | 26 | # Radius-based spatial subsampling 27 | def uniform_sample(arr,radius): 28 | candidates = np.array(arr).copy() 29 | locations = np.arange(len(candidates)) 30 | 31 | result = [] 32 | result_idx = [] 33 | 34 | while candidates.size > 0: 35 | remove = [] 36 | rows = len(candidates) 37 | sample = candidates[0] 38 | index = np.arange(rows).reshape(-1,1) 39 | dists = np.column_stack((index,candidates)) 40 | result.append(sample) 41 | result_idx.append(locations[0]) 42 | for row in dists: 43 | if la.norm(row[1:] - sample) < radius: 44 | remove.append(int(row[0])) 45 | candidates = np.delete(candidates, remove, axis=0) 46 | locations = np.delete(locations, remove) 47 | return np.array(result), np.array(result_idx) 48 | 49 | 50 | def huber_loss(x,c): 51 | if abs(x) <= c: 52 | return 0.5 * (x**2) 53 | else: 54 | return c * (abs(x) - 0.5*c) 55 | 56 | def tukey_biweight_loss(x,c): 57 | if abs(x) > c: 58 | return 0 59 | else: 60 | return x * (1 - (x/c)**2)**2 61 | 62 | 63 | ''' 64 | Transform a 3D point directly with a dual quaternion. 65 | A 3-vector (x,y,z) can be expresed by vq = 1 + epsilon(xi + yj + zk), or [1,0,0,0,0,x,y,z] 66 | The transformation of it by a dq is given by dq*v*(quaternion and dual conjugate of dq) 67 | ''' 68 | def dqb_warp(dq, pos): 69 | vq = np.array([1,0,0,0,0,pos[0],pos[1],pos[2]], dtype=np.float32) 70 | dqv = dual_quaternion_multiply(dq,vq) 71 | dqvdqc = dual_quaternion_multiply(dqv,dual_quaternion_conjugate(dq)) 72 | return dqvdqc[-3:] 73 | 74 | def dqb_warp_normal(dq,pos): 75 | rq = np.append(dq[:4],[0,0,0,0]) 76 | return dqb_warp(rq,pos) 77 | 78 | # basis of the 8 vector dual quaternion is (1, i, j, k, e, ei, ej, ek) 79 | def SE3TDQ(M): 80 | R, t = decompose_se3(M) 81 | q = quaternion_from_matrix(M) 82 | q = q / la.norm(q) 83 | qe = 0.5 * quaternion_multiply([0,t[0],t[1],t[2]],q) 84 | return np.append(q,qe) 85 | 86 | def DQTSE3(q): 87 | R = quaternion_matrix(q[:4])[np.ix_([0,1,2],[0,1,2])] 88 | t = quaternion_multiply(2 * q[4:], quaternion_conjugate(q[:4])) 89 | return compose_se3(R,t[1:]) 90 | 91 | def InitialSE3(): 92 | M = np.identity(4) 93 | M[0,0] = 0.9 94 | M[1,1] = 0.9 95 | M[2,2] = 0.9 96 | M[0,3] = 0.1 97 | M[1,3] = 0.1 98 | M[2,3] = 0.1 99 | return M 100 | 101 | # Trilinear interpolation of signed distance. Return None if pos is out of the volume. 102 | def interpolate_tsdf(pos, tsdf): 103 | if tsdf.ndim != 3: 104 | raise ValueError('Only 3D numpy array is accepted') 105 | res_x, res_y, res_z = tsdf.shape 106 | 107 | if min(pos) < 0 or pos[0] > res_x - 1 or pos[1] > res_y - 1 or pos[2] > res_z - 1 : 108 | return None 109 | 110 | x0 = math.floor(pos[0]) 111 | y0 = math.floor(pos[1]) 112 | z0 = math.floor(pos[2]) 113 | x1 = math.ceil(pos[0]) 114 | y1 = math.ceil(pos[1]) 115 | z1 = math.ceil(pos[2]) 116 | 117 | xd = pos[0] - x0 118 | yd = pos[1] - y0 119 | zd = pos[2] - z0 120 | 121 | c000 = tsdf[(x0,y0,z0)] 122 | c100 = tsdf[(x1,y0,z0)] 123 | c001 = tsdf[(x0,y1,z0)] 124 | c101 = tsdf[(x1,y1,z0)] 125 | c010 = tsdf[(x0,y0,z1)] 126 | c110 = tsdf[(x1,y0,z1)] 127 | c011 = tsdf[(x0,y1,z1)] 128 | c111 = tsdf[(x1,y1,z1)] 129 | 130 | c00 = c000 * (1-xd) + c100 * xd 131 | c01 = c001 * (1-xd) + c101 * xd 132 | c10 = c010 * (1-xd) + c110 * xd 133 | c11 = c011 * (1-xd) + c111 * xd 134 | 135 | c0 = c00 * (1-yd) + c10 * yd 136 | c1 = c01 * (1-yd) + c11 * yd 137 | return c0 * (1-zd) + c1 * zd 138 | 139 | def cal_dist(a,b): 140 | return la.norm(a-b) 141 | 142 | 143 | def quaternion_matrix(quaternion): 144 | """Return homogeneous rotation matrix from quaternion. 145 | 146 | >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) 147 | >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) 148 | True 149 | >>> M = quaternion_matrix([1, 0, 0, 0]) 150 | >>> numpy.allclose(M, numpy.identity(4)) 151 | True 152 | >>> M = quaternion_matrix([0, 1, 0, 0]) 153 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 154 | True 155 | 156 | """ 157 | q = np.array(quaternion, dtype=np.float64, copy=True) 158 | n = np.dot(q, q) 159 | if n < _EPS: 160 | return np.identity(4) 161 | q *= math.sqrt(2.0 / n) 162 | q = np.outer(q, q) 163 | return np.array([ 164 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 165 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 166 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 167 | [ 0.0, 0.0, 0.0, 1.0]]) 168 | 169 | 170 | def quaternion_from_matrix(matrix, isprecise=False): 171 | """Return quaternion from rotation matrix. 172 | 173 | If isprecise is True, the input matrix is assumed to be a precise rotation 174 | matrix and a faster algorithm is used. 175 | 176 | >>> q = quaternion_from_matrix(numpy.identity(4), True) 177 | >>> numpy.allclose(q, [1, 0, 0, 0]) 178 | True 179 | >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) 180 | >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) 181 | True 182 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 183 | >>> q = quaternion_from_matrix(R, True) 184 | >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) 185 | True 186 | >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], 187 | ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] 188 | >>> q = quaternion_from_matrix(R) 189 | >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) 190 | True 191 | >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], 192 | ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] 193 | >>> q = quaternion_from_matrix(R) 194 | >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) 195 | True 196 | >>> R = random_rotation_matrix() 197 | >>> q = quaternion_from_matrix(R) 198 | >>> is_same_transform(R, quaternion_matrix(q)) 199 | True 200 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False), 201 | ... quaternion_from_matrix(R, isprecise=True)) 202 | True 203 | >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0) 204 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False), 205 | ... quaternion_from_matrix(R, isprecise=True)) 206 | True 207 | 208 | """ 209 | M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4] 210 | if isprecise: 211 | q = np.empty((4, )) 212 | t = np.trace(M) 213 | if t > M[3, 3]: 214 | q[0] = t 215 | q[3] = M[1, 0] - M[0, 1] 216 | q[2] = M[0, 2] - M[2, 0] 217 | q[1] = M[2, 1] - M[1, 2] 218 | else: 219 | i, j, k = 0, 1, 2 220 | if M[1, 1] > M[0, 0]: 221 | i, j, k = 1, 2, 0 222 | if M[2, 2] > M[i, i]: 223 | i, j, k = 2, 0, 1 224 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 225 | q[i] = t 226 | q[j] = M[i, j] + M[j, i] 227 | q[k] = M[k, i] + M[i, k] 228 | q[3] = M[k, j] - M[j, k] 229 | q = q[[3, 0, 1, 2]] 230 | q *= 0.5 / math.sqrt(t * M[3, 3]) 231 | else: 232 | m00 = M[0, 0] 233 | m01 = M[0, 1] 234 | m02 = M[0, 2] 235 | m10 = M[1, 0] 236 | m11 = M[1, 1] 237 | m12 = M[1, 2] 238 | m20 = M[2, 0] 239 | m21 = M[2, 1] 240 | m22 = M[2, 2] 241 | # symmetric matrix K 242 | K = np.array([[m00-m11-m22, 0.0, 0.0, 0.0], 243 | [m01+m10, m11-m00-m22, 0.0, 0.0], 244 | [m02+m20, m12+m21, m22-m00-m11, 0.0], 245 | [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) 246 | K /= 3.0 247 | # quaternion is eigenvector of K that corresponds to largest eigenvalue 248 | w, V = np.linalg.eigh(K) 249 | q = V[[3, 0, 1, 2], np.argmax(w)] 250 | if q[0] < 0.0: 251 | np.negative(q, q) 252 | return q 253 | 254 | 255 | def quaternion_multiply(quaternion1, quaternion0): 256 | """Return multiplication of two quaternions. 257 | 258 | >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) 259 | >>> np.allclose(q, [28, -44, -14, 48]) 260 | True 261 | 262 | """ 263 | w0, x0, y0, z0 = quaternion0 264 | w1, x1, y1, z1 = quaternion1 265 | return np.array([ 266 | -x1*x0 - y1*y0 - z1*z0 + w1*w0, 267 | x1*w0 + y1*z0 - z1*y0 + w1*x0, 268 | -x1*z0 + y1*w0 + z1*x0 + w1*y0, 269 | x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=np.float64) 270 | 271 | ''' 272 | Let q = qr + qd*eps 273 | q1*q2 = qr1*qr2 + (qr1*qd2 + qd1*qr2)epsilon 274 | ''' 275 | def dual_quaternion_multiply(q1,q2): 276 | qr1 = q1[:4] 277 | qd1 = q1[4:] 278 | qr2 = q2[:4] 279 | qd2 = q2[4:] 280 | qr = quaternion_multiply(qr1,qr2) 281 | qd = quaternion_multiply(qr1,qd2) + quaternion_multiply(qd1,qr2) 282 | return np.append(qr,qd) 283 | 284 | 285 | def quaternion_conjugate(quaternion): 286 | """Return conjugate of quaternion. 287 | 288 | >>> q0 = random_quaternion() 289 | >>> q1 = quaternion_conjugate(q0) 290 | >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) 291 | True 292 | 293 | """ 294 | q = np.array(quaternion, dtype=np.float64, copy=True) 295 | np.negative(q[1:], q[1:]) 296 | return q 297 | 298 | 299 | def dual_quaternion_conjugate(dquaternion): 300 | dq = np.array(dquaternion, dtype=np.float64, copy=True) 301 | np.negative(dq[4:],dq[4:]) 302 | np.negative(dq[5:],dq[5:]) 303 | np.negative(dq[1:4],dq[1:4]) 304 | return dq 305 | 306 | 307 | ''' 308 | lw: 3x4 camera extrinsic 309 | K: 3x3 camera intrinsic 310 | pos: (x,y,z) 311 | ''' 312 | def project_to_pixel(K, pos, lw = None): 313 | p = [] 314 | if lw is None: 315 | if len(pos) == 4: 316 | pos = pos[:-1] 317 | p = np.matmul(K,pos) 318 | if p[2] == 0: 319 | return (None, None) 320 | return (p[0]/p[2], p[1]/p[2]) 321 | else: 322 | if len(pos) == 3: 323 | pos = np.append(pos,1) 324 | p = np.matmul(K, np.matmul(lw,pos)) 325 | if p[2] == 0: 326 | return (None, None) 327 | else: 328 | return (p[0]/p[2],p[1]/p[2]) 329 | 330 | def read_proj_matrix(fpath): 331 | f = open(fpath,'r') 332 | arr = [] 333 | for line in f: 334 | arr.append(line[:-1].split(' ')) 335 | return np.array(arr,dtype='float') 336 | 337 | # find the inverse of a 3x4 rigid transformation matrix 338 | def inverse_rigid_matrix(A): 339 | R,t = decompose_se3(A) 340 | R_inv = la.inv(R) 341 | t_inv = np.matmul(R_inv,t) * -1 342 | M = np.zeros((3,4)) 343 | M[0] = np.append(R_inv[0],t_inv[0]) 344 | M[1] = np.append(R_inv[1],t_inv[1]) 345 | M[2] = np.append(R_inv[2],t_inv[2]) 346 | return M 347 | -------------------------------------------------------------------------------- /core/fusion.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Dynamic fusion from a live frame at t to the canonical space S. 3 | The steps are: estimation of warp field -> tsdf fusion -> update deformation nodes 4 | The warp field is associated with a set of warp nodes N={dg_v, dg_w, dg_SE} 5 | The warp nodes are simply subsampled from canonical frame vertices 6 | 7 | Once the SE(3) for each warp node is computed, the warp field provides a warp function W(x) which transforms a point x in the canonical space to a point in the live frame. 8 | 9 | The Fusion object is initialized with a truncated signed distance volumne of the first frame (treated as the canonical frame). 10 | To process a new frame, we need two things: tsdf of the new frame and correponding points. The processing of a new frame is done by something like: 11 | 12 | fusion.solve(corr_to_current_frame) 13 | fusion.updateTSDF(tsdf_of_current_frame) 14 | fusion.update_graph() 15 | 16 | 17 | All input/output datatypes should be be numpy arrays. 18 | 19 | Important hyperparameters: 20 | - subsample_rate: determine the density of deformation nodes embedded in the canonical mesh 21 | - marching_cubes_step_size: determine the quality of the extracted mesh from marching cube 22 | - knn: affect blending and regularization. 23 | - wmax: maximum weight that can be accumulated in tsdf update. Affect the influences of the later fused frames. 24 | - method for finding correspondences 25 | 26 | TODO: 27 | - optimizer is too slow. Most time spent on Jacobian estimation 28 | - (Optional) provide an option to calculate dmax for DQB weight 29 | 30 | 31 | ''' 32 | 33 | import math 34 | import numpy as np 35 | import pickle 36 | import os 37 | from numpy import linalg as la 38 | from scipy.spatial import KDTree 39 | from scipy.optimize import least_squares 40 | from scipy.sparse import lil_matrix 41 | from skimage import measure 42 | from .util import * 43 | from .sdf import * 44 | from . import * 45 | 46 | 47 | DATA_PATH = os.path.join(os.path.dirname(os.path.dirname(os.path.realpath(__file__))), 'data') 48 | 49 | class Fusion: 50 | def __init__(self, trunc_distance, subsample_rate = 5.0, knn = 4, marching_cubes_step_size = 3, verbose = False, use_cnn = True, write_warpfield = True): 51 | if type(tsdf) is not np.ndarray or tsdf.ndim != 3: 52 | raise ValueError('Only 3D numpy array is accepted as tsdf') 53 | 54 | self._itercounter = 0 55 | self._curr_tsdf = None 56 | self._tdist = abs(trunc_distance) 57 | self._lw = np.array([1,0,0,0,0,0.1,0,0],dtype=np.float32) 58 | self._knn = knn 59 | self._marching_cubes_step_size = marching_cubes_step_size 60 | self._nodes = [] 61 | self._neighbor_look_up = [] 62 | self._correspondences = [] 63 | self._kdtree = None 64 | self._verbose = verbose 65 | self._write_warpfield = write_warpfield 66 | 67 | if use_cnn: 68 | self.input, self._feature, self._sess = cnnInitialize() 69 | else: 70 | self._sess = None 71 | 72 | 73 | def InitializeCanonicalSpace(self, tsdf = None,depths = None, lws = None, K = None, tsdf_size = 256): 74 | self._tsdfw = np.zeros(tsdf.shape) 75 | if tsdf is not None: 76 | self._tsdf = tsdf 77 | elif depths is not None and depths is not None and lws is not None and K is not None: 78 | self._K = K 79 | self._Kinv = la.inv(K) 80 | self._tsdf = np.zeros((tsdf_size,tsdf_size,tsdf_size)) + self._tdist 81 | for i in len(depths): 82 | if verbose: 83 | print('Fusing %d depth map'%(i)) 84 | fuseDepths(depths[i],lws[i],self._tsdf, self._tsdfw) 85 | 86 | if verbose: 87 | print("Running initial marching cubes") 88 | self.marching_cubes() 89 | average_distances = [] 90 | for f in self._faces: 91 | average_distances.append(self.average_edge_dist_in_face(f)) 92 | self._radius = subsample_rate * np.average(np.array(average_distances)) 93 | 94 | if verbose: 95 | print("Constructing initial graph...") 96 | self.construct_graph() 97 | 98 | 99 | 100 | # Construct deformation graph from canonical vertices 101 | def construct_graph(self): 102 | # uniform sampling 103 | nodes_v, nodes_idx = uniform_sample(self._vertices,self._radius) 104 | if self._verbose: 105 | print("%d deformation nodes sampled, with average radius of %f" % (len(nodes_v), self._radius)) 106 | 107 | ''' 108 | Each node is a 4-tuple (index of corresponding surface vertex dg_idx, 3D position dg_v, 4x4 Transformation dg_se3, weight dg_w) 109 | HackHack: 110 | Not sure how to determine dgw. Use sample radius for now. 111 | ''' 112 | for i in range(len(nodes_v)): 113 | self._nodes.append((nodes_idx[i], 114 | nodes_v[i], 115 | np.array([1,0.00,0.00,0.00,0.00,0.01,0.01,0.00],dtype=np.float32), 116 | 2 * self._radius)) 117 | 118 | # construct kd tree 119 | self._kdtree = KDTree(nodes_v) 120 | self._neighbor_look_up = [] 121 | for vert in self._vertices: 122 | dists, idx = self._kdtree.query(vert, k=self._knn) 123 | self._neighbor_look_up.append(idx) 124 | 125 | 126 | # fuse a depth map into current tsdf 127 | def fuseDepths(self, dm, lw, tsdf, tsdfw, wmax = 100.0): 128 | (dmx,dmy) = current_dm.shape 129 | itest = 0 130 | it = np.nditer(tsdf, flags=['multi_index'], op_flags = ['readwrite']) 131 | while not it.finished: 132 | tsdf_s = np.copy(it[0]) 133 | pos = np.array(it.multi_index, dtype=np.float32) 134 | (u,v) = project_to_pixel(lw, self._K, pos) 135 | if u >= 0 and u < dmx and v >= 0 and v < dmy: 136 | uc = current_dm[round(u)][round(v)] * np.array([u,v,1]) 137 | # signed distance along principal axis of the camera 138 | tsdf_l = (np.matmul(self._Kinv,uc))[2] - pos[2] 139 | 140 | # TODO: weight here does not make a lot of sense 141 | if tsdf_l > -1 * self._tdist: 142 | wi = 1 143 | wi_t = tsdfw[it.multi_index] 144 | # Update (v(x),w(x)) 145 | it[0] = (it[0] * wi_t + min(self._tdist, tsdf_l)*wi)/(wi + wi_t) 146 | tsdfw[it.multi_index] = min(wi + wi_t, wmax) 147 | 148 | itest += 1 149 | it.iternext() 150 | 151 | 152 | # Perform surface fusion for each voxel center with a tsdf query function for the live frame. 153 | def updateTSDF(self, curr_tsdf = None, wmax = 100.0): 154 | 155 | if curr_tsdf is not None: 156 | self._curr_tsdf = curr_tsdf 157 | 158 | if self._curr_tsdf is None: 159 | raise ValueError('tsdf of live frame has not been loaded') 160 | if type(self._curr_tsdf) is not np.ndarray: 161 | raise ValueError('Only accept 3D np array as tsdf') 162 | elif self._curr_tsdf.ndim != 3: 163 | raise ValueError('Only accept 3D np array as tsdf') 164 | 165 | ''' 166 | if self._curr_tsdf.shape != self._tsdf.shape: 167 | raise ValueError('live frame TSDF should match the size of canonical TSDF') 168 | ''' 169 | itest = 0 170 | 171 | it = np.nditer(self._tsdf, flags=['multi_index'], op_flags = ['readwrite']) 172 | while not it.finished: 173 | tsdf_s = np.copy(it[0]) 174 | pos = np.array(it.multi_index, dtype=np.float32) 175 | dists, kdidx = self._kdtree.query(pos, k=self._knn + 1) 176 | locations = kdidx[:-1] 177 | matrices = [self._nodes[i][2] for i in locations] 178 | tsdf_l = interpolate_tsdf(self.warp(pos, matrices, locations, m_lw = self._lw), self._curr_tsdf) 179 | if tsdf_l is not None and tsdf_l > -1 * self._tdist: 180 | wi = 0 181 | wi_t = self._tsdfw[it.multi_index] 182 | for idx in locations: 183 | wi += la.norm(self._nodes[idx][1] - pos) / len(locations) 184 | # Update (v(x),w(x)) 185 | 186 | if wi_t == 0: 187 | wi_t = wi 188 | 189 | it[0] = (it[0] * wi_t + min(self._tdist, tsdf_l)*wi)/(wi + wi_t) 190 | self._tsdfw[it.multi_index] = min(wi + wi_t, wmax) 191 | 192 | if itest % 250 == 0 and itest < 5000: 193 | print('original tsdf and weight: (%f,%f). new tsdf and weight: (%f,%f)'%(tsdf_s, wi_t, it[0], min(wi + wi_t, wmax))) 194 | print('interpolated tsdf at warp location:',tsdf_l) 195 | print('new weight vs original weight:',wi,wi_t) 196 | 197 | itest += 1 198 | it.iternext() 199 | 200 | # Update the deformation graph after new surafce vertices are found 201 | def update_graph(self): 202 | self.marching_cubes() 203 | # update topology of existing nodes 204 | vert_kdtree = KDTree(self._vertices) 205 | for i in range(len(self._nodes)): 206 | pos = self._nodes[i][1] 207 | se3 = self._nodes[i][2] 208 | dist, vidx = vert_kdtree.query(pos) 209 | self._nodes[i] = (vidx, pos, se3, 2*self._radius) 210 | 211 | # find unsupported surface points 212 | unsupported_vert = [] 213 | for vert in self._vertices: 214 | dists, kdidx = self._kdtree.query(vert,k=self._knn) 215 | if min([ la.norm(self._nodes[idx][1] - vert)/self._nodes[idx][3] for idx in kdidx]) >= 1: 216 | unsupported_vert.append(vert) 217 | 218 | nodes_new_v, nodes_new_idx = uniform_sample(unsupported_vert, self._radius) 219 | for i in range(len(nodes_new_v)): 220 | self._nodes.append((nodes_new_idx[i], 221 | nodes_new_v[i], 222 | self.dq_blend(nodes_new_v[i]), 223 | 2 * self._radius)) 224 | 225 | if self._verbose: 226 | print("Inserted %d new deformation nodes. Current number of deformation nodes: %d" % (len(nodes_new_v), len(self._nodes))) 227 | 228 | # recompute KDTree and neighbors 229 | self._kdtree = KDTree(np.array([n[1] for n in self._nodes])) 230 | self._neighbor_look_up = [] 231 | for vert in self._vertices: 232 | dists, idx = self._kdtree.query(vert, k=self._knn) 233 | self._neighbor_look_up.append(idx) 234 | 235 | # since fusion is complete at this point, delete current live frame data 236 | self._curr_tsdf = None 237 | self._correspondences = [] 238 | if self._write_warpfield: 239 | self.write_warp_field(DATA_PATH, 'test') 240 | 241 | 242 | 243 | def setupCorrespondences(self, curr_tsdf, method = 'cnn', prune_result = True, tolerance = 0.2): 244 | self._curr_tsdf = curr_tsdf 245 | self._correspondences = [] 246 | idx_pruned = [] 247 | lverts, lfaces, lnormals, lvalues = self.marching_cubes(curr_tsdf, step_size = 1) 248 | 249 | print('lverts shape:',lverts.shape) 250 | 251 | if self._sess is None or method == 'clpts': 252 | if self._verbose: 253 | print('Using closest pts method for finding correspondences...') 254 | 255 | l_kdtree = KDTree(lverts) 256 | i = 0 257 | 258 | for idx in range(len(self._vertices)): 259 | locations = self._neighbor_look_up[idx] 260 | knn_dqs = [self._nodes[i][2] for i in locations] 261 | v_warped, n_warped = self.warp(self._vertices[idx],knn_dqs,locations,self._normals[idx], m_lw = self._lw) 262 | dists, iidx = l_kdtree.query(v_warped,k=self._knn) 263 | best_pt = lverts[iidx[0]] 264 | best_cost = 1 265 | 266 | 267 | for idx in iidx: 268 | p = lverts[idx] 269 | cost = abs(np.dot(n_warped, v_warped - p)) 270 | if cost < best_cost: 271 | best_cost = cost 272 | best_pt = p 273 | if best_cost > tolerance: 274 | idx_pruned.append(idx) 275 | i+=1 276 | self._correspondences.append(best_pt) 277 | else: 278 | if self._verbose: 279 | print('Using cnn method for finding correspondences...') 280 | s_feats = compute_correspondence(self.input, self._feature, self._sess, self._vertices, self._faces) 281 | l_feats = compute_correspondence(self.input, self._feature, self._sess, lverts,lfaces) 282 | l_kdtree = KDTree(np.array(l_feats)) 283 | for idx in range(len(s_feats)): 284 | dists, iidx = l_kdtree.query(s_feats[idx]) 285 | self._correspondences.append(lverts[iidx]) 286 | 287 | if prune_result: 288 | if method == 'cnn': 289 | # Prune out bad correspondences 290 | for idx in range(len(self._vertices)): 291 | locations = self._neighbor_look_up[idx] 292 | knn_dqs = [self._nodes[i][2] for i in locations] 293 | v_warped, n_warped = self.warp(self._vertices[idx],knn_dqs,locations,self._normals[idx], m_lw = self._lw) 294 | cost = abs(np.dot(n_warped,v_warped - self._correspondences[idx])) 295 | if cost > tolerance: 296 | idx_pruned.append(idx) 297 | 298 | if self._verbose: 299 | print('ratio of correspondence outlier rejection', float(len(idx_pruned))/float(len(self._vertices))) 300 | 301 | # update data (HACKHACK) 302 | self._vertices = np.delete(self._vertices, idx_pruned, axis=0) 303 | self._correspondences = np.delete(self._correspondences, idx_pruned, axis=0) 304 | self._neighbor_look_up = np.delete(self._neighbor_look_up, idx_pruned, axis=0) 305 | self._normals = np.delete(self._normals, idx_pruned, axis=0) 306 | self._faces = None 307 | 308 | vert_kdtree = KDTree(self._vertices) 309 | for i in range(len(self._nodes)): 310 | pos = self._nodes[i][1] 311 | se3 = self._nodes[i][2] 312 | dist, vidx = vert_kdtree.query(pos) 313 | self._nodes[i] = (vidx, pos, se3, 2*self._radius) 314 | 315 | 316 | 317 | # Solve for a warp field {dg_SE} with correspondences to the live frame 318 | ''' 319 | Correspondences format: A list of 3D corresponding points in live frame. 320 | Length should equal the length of surface vertices (self._vertices). 321 | The indices should also match with the surface vertices array indices. 322 | 323 | E = Data_term(Warp Field, surface points, surface normals) + Regularization_term(Warp Field, deformation graph) 324 | Nonlinear least square problem. The paper solved it by Iterative Gauss-Newton with a Sparse Cholesky Solver. 325 | how about scipy.optimize.least_squares? 326 | ''' 327 | def solve(self, 328 | correspondences = None, 329 | method = 'cnn', 330 | precompute_lw = True, 331 | tukey_data_weight = 0.2, 332 | huber_regularization_weight = 0.001, 333 | regularization_weight = 1): 334 | 335 | if correspondences is not None: 336 | self._correspondences = correspondences 337 | if len(self._correspondences) != len(self._vertices): 338 | raise ValueError("Please first call setupCorrespondences to compute point to point correspondences between canonical and live frame vertices!") 339 | 340 | iteration = 1 341 | if method == 'clpts': 342 | iteration = 3 343 | 344 | self._itercounter += 1 345 | solver_verbose_level = 0 346 | if self._verbose: 347 | solver_verbose_level = 2 348 | 349 | # We may consider using other optimization library 350 | if precompute_lw: 351 | if self._verbose: 352 | print('estimating global transformation lw...') 353 | 354 | for iter in range(1): 355 | values = self._lw 356 | opt_result = least_squares(self.computef_lw, 357 | values, 358 | max_nfev = 100, 359 | verbose=solver_verbose_level, 360 | args=(tukey_data_weight, 1)) 361 | print('global transformation found:', DQTSE3(opt_result.x)) 362 | self._lw = opt_result.x 363 | if method == 'clpts': 364 | self.setupCorrespondences(self._curr_tsdf, method = 'clpts') 365 | 366 | if self._verbose: 367 | print('estimating warp field...') 368 | for iter in range(iteration): 369 | 370 | if iter > 0 and correspondences is None: 371 | self.setupCorrespondences(self._curr_tsdf, method = 'clpts') 372 | 373 | values = np.concatenate([ dg[2] for dg in self._nodes], axis=0).flatten() 374 | n = len(self._vertices) + 3 * self._knn * len(self._nodes) 375 | f = self.computef(values,tukey_data_weight,huber_regularization_weight, regularization_weight) 376 | cost_before = 0.5 * np.inner(f,f) 377 | 378 | if self._verbose: 379 | print("Cost before optimization:",cost_before) 380 | print('Current regularization weight:',regularization_weight) 381 | 382 | opt_result = least_squares(self.computef, 383 | values, 384 | method='trf', 385 | jac='2-point', 386 | ftol=1e-5, 387 | tr_solver='lsmr', 388 | jac_sparsity = self.computeSparsity(n, len(values)), 389 | loss = 'huber', 390 | max_nfev = 20, 391 | verbose = solver_verbose_level, 392 | args=(tukey_data_weight, huber_regularization_weight, regularization_weight)) 393 | # Results: (x, cost, fun, jac, grad, optimality) 394 | new_values = opt_result.x 395 | if self._verbose: 396 | diff = la.norm(new_values - values) 397 | print("Optimized cost at %d iteration: %f" % (self._itercounter, opt_result.cost)) 398 | print("Norm of displacement (total): %f; sum: %f" % (diff, (new_values - values).sum())) 399 | 400 | nw_dqs = np.split(new_values,len(new_values)/8) 401 | for idx in range(len(self._nodes)): 402 | nd = self._nodes[idx] 403 | self._nodes[idx] = (nd[0], nd[1], nw_dqs[idx], nd[3]) 404 | 405 | reduct_rate = (cost_before - opt_result.cost)/cost_before 406 | # relax regularization 407 | if reduct_rate > 0.05 and reduct_rate < 0.9: 408 | regularization_weight /= 8 409 | if self._verbose: 410 | print('Cost reduction rate:', reduct_rate) 411 | else: 412 | break 413 | 414 | 415 | # Optional: we can compute a sparsity structure to speed up the optimizer 416 | def computeSparsity(self, n, m): 417 | sparsity = lil_matrix((n,m), dtype=np.float32) 418 | ''' 419 | fill non-zero entries with 1 420 | ''' 421 | data_term_length = len(self._vertices) 422 | 423 | for idx in range(data_term_length): 424 | locations = self._neighbor_look_up[idx] 425 | for loc in locations: 426 | for i in range(8): 427 | sparsity[idx, 8 * loc + i] = 1 428 | 429 | for idx in range(len(self._nodes)): 430 | for i in range(8): 431 | sparsity[data_term_length + 3*idx, 8 * (idx) + i] = 1 432 | sparsity[data_term_length + 3*idx + 1, 8 * (idx) + i] = 1 433 | sparsity[data_term_length + 3*idx + 2, 8 * (idx) + i] = 1 434 | 435 | for nidx in self._neighbor_look_up[self._nodes[idx][0]]: 436 | for i in range(8): 437 | sparsity[data_term_length + 3*idx, 8 * (nidx) + i] = 1 438 | sparsity[data_term_length + 3*idx + 1, 8 * (nidx) + i] = 1 439 | sparsity[data_term_length + 3*idx + 2, 8 * (nidx) + i] = 1 440 | 441 | 442 | return sparsity 443 | 444 | def computef_lw(self,x,tdw,trw): 445 | f = [] 446 | 447 | # Data Term only 448 | for idx in range(len(self._vertices)): 449 | locations = self._neighbor_look_up[idx] 450 | knn_dqs = [self._nodes[i][2] for i in locations] 451 | vert_warped, n_warped = self.warp(self._vertices[idx], knn_dqs, locations, self._normals[idx], m_lw = x) 452 | p2s = np.dot(n_warped, vert_warped - self._correspondences[idx]) 453 | #f.append(np.sign(p2s) * math.sqrt(tukey_biweight_loss(abs(p2s),tdw))) 454 | f.append(p2s) 455 | 456 | return np.array(f) 457 | 458 | # Compute residual function. Input is a flattened vector {dg_SE3} 459 | def computef(self, x, tdw, trw, rw): 460 | f = [] 461 | dqs = np.split(x, len(x)/8) 462 | 463 | dte = 0 464 | rte = 0 465 | # Data Term 466 | for idx in range(len(self._vertices)): 467 | locations = self._neighbor_look_up[idx] 468 | knn_dqs = [dqs[i] for i in locations] 469 | vert_warped, n_warped = self.warp(self._vertices[idx], knn_dqs, locations, self._normals[idx], m_lw = self._lw) 470 | p2s = np.dot(n_warped, vert_warped - self._correspondences[idx]) 471 | f.append(p2s) 472 | #f.append(np.sign(p2s) * math.sqrt(tukey_biweight_loss(abs(p2s),tdw))) 473 | dte += f[-1]**2 474 | # Regularization Term: Instead of regularization tree, just use the simpler knn nodes for now 475 | for idx in range(len(self._nodes)): 476 | dgi_se3 = dqs[idx] 477 | for nidx in self._neighbor_look_up[self._nodes[idx][0]]: 478 | dgj_v = self._nodes[nidx][1] 479 | dgj_se3 = dqs[nidx] 480 | diff = dqb_warp(dgi_se3,dgj_v) - dqb_warp(dgj_se3, dgj_v) 481 | for i in range(3): 482 | f.append(rw * max(self._nodes[idx][3], self._nodes[nidx][3]) * diff[i]) 483 | #f.append(np.sign(diff[i]) * math.sqrt(rw * max(self._nodes[idx][3], self._nodes[nidx][3]) * huber_loss(diff[i], trw))) 484 | rte += f[-1]**2 485 | 486 | ''' 487 | if self._verbose: 488 | print("Data term energy:%f; reg term energy:%f"%(dte,rte)) 489 | ''' 490 | 491 | return np.array(f) 492 | 493 | ''' 494 | Warp a point from canonical space to the current live frame, using the wrap field computed from t-1. No camera matrix needed. 495 | params: 496 | dq: dual quaternions {dg_SE3} used for dual quaternion blending 497 | locations: indices for corresponding node in the graph. 498 | normal: if provided, return a warped normal as well. 499 | dmax: if provided, use the value instead of dgw to calculate weights 500 | m_lw: if provided, apply global rigid transformation 501 | ''' 502 | def warp(self, pos, dqs = None, locations = None, normal = None, dmax = None, m_lw = None): 503 | if dqs is None or locations is None: 504 | dists, kdidx = self._kdtree.query(pos, k=self._knn+1) 505 | locations = kdidx[:-1] 506 | dqs = [self._nodes[i][2] for i in locations] 507 | 508 | se3 = self.dq_blend(pos,dqs,locations,dmax) 509 | 510 | pos_warped = dqb_warp(se3,pos) 511 | if m_lw is not None: 512 | pos_warped = dqb_warp(m_lw, pos_warped) 513 | 514 | if normal is not None: 515 | normal_warped = dqb_warp_normal(se3, normal) 516 | if m_lw is not None: 517 | normal_warped = dqb_warp_normal(m_lw, normal_warped) 518 | return (pos_warped,normal_warped) 519 | else: 520 | return pos_warped 521 | 522 | ''' 523 | Not sure how to determine dgw, so following idea from [Sumner 07] to calculate weights in DQB. 524 | The idea is to use the knn + 1 node as a denominator. 525 | ''' 526 | # Interpolate a se3 matrix from k-nearest nodes to pos. 527 | def dq_blend(self, pos, dqs = None, locations = None, dmax = None): 528 | if dqs is None or locations is None: 529 | dists, locations = self._kdtree.query(pos, k=self._knn) 530 | dqs = [self._nodes[i][2] for i in locations] 531 | 532 | dqb = np.zeros(8) 533 | for idx in range(len(dqs)): 534 | dg_idx, dg_v, dg_se3, dg_w = self._nodes[locations[idx]] 535 | dg_dq = dqs[idx] 536 | if dmax is None: 537 | w = math.exp( -1.0 * (la.norm(pos - dg_v)/(2*dg_w))**2) 538 | dqb += w * dg_dq 539 | else: 540 | w = math.exp( -1.0 * (la.norm(pos - dg_v)/dmax)**2) 541 | dqb += w * dg_dq 542 | 543 | #Hackhack 544 | if la.norm(dqb) == 0: 545 | if self._verbose: 546 | print('Really weird thing just happend!!!! blended dq is a zero vector') 547 | print('dqs:', dqs) 548 | print('locations:', locations) 549 | return np.array([1,0,0,0,0,0,0,0],dtype=np.float32) 550 | 551 | return dqb / la.norm(dqb) 552 | 553 | # Mesh vertices and normal extraction from current tsdf in canonical space 554 | def marching_cubes(self, tsdf = None, step_size = 0): 555 | 556 | if step_size < 1: 557 | step_size = self._marching_cubes_step_size 558 | 559 | if tsdf is not None: 560 | return measure.marching_cubes_lewiner(tsdf, 561 | step_size = step_size, 562 | allow_degenerate=False) 563 | 564 | self._vertices, self._faces, self._normals, values = measure.marching_cubes_lewiner(self._tsdf, 565 | step_size = step_size, 566 | allow_degenerate=False) 567 | if self._verbose: 568 | print("Marching Cubes result: number of extracted vertices is %d" % (len(self._vertices))) 569 | 570 | # Write the current warp field to file 571 | def write_warp_field(self, path, filename): 572 | file = open( os.path.join(path, filename + '__' + str(self._itercounter) + '.p'),'wb') 573 | pickle.dump(self._nodes, file) 574 | 575 | 576 | # Write the canonical mesh to file 577 | def write_canonical_mesh(self, path, filename): 578 | fpath = open(os.path.join(path,filename),'w') 579 | verts, faces, normals, values = measure.marching_cubes_lewiner(self._tsdf, allow_degenerate=False) 580 | for v in verts: 581 | fpath.write('v %f %f %f\n'%(v[0],v[1],v[2])) 582 | for n in normals: 583 | fpath.write('vn %f %f %f\n'%(n[0],n[1],n[2])) 584 | for f in faces: 585 | fpath.write('f %d %d %d\n'%(f[0] + 1,f[1] + 1,f[2] + 1)) 586 | fpath.close() 587 | 588 | # Process a warp field file and write the live frame mesh 589 | def write_live_frame_mesh(self,path,filename, warpfield_path): 590 | pass 591 | 592 | def average_edge_dist_in_face(self, f): 593 | v1 = self._vertices[f[0]] 594 | v2 = self._vertices[f[1]] 595 | v3 = self._vertices[f[2]] 596 | return (cal_dist(v1,v2) + cal_dist(v1,v3) + cal_dist(v2,v3))/3 597 | 598 | 599 | -------------------------------------------------------------------------------- /core/fusion_dm.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Dynamic fusion from a live frame at t to the canonical space S. 3 | The steps are: estimation of warp field -> tsdf fusion -> update deformation nodes 4 | The warp field is associated with a set of warp nodes N={dg_v, dg_w, dg_SE} 5 | The warp nodes are simply subsampled from canonical frame vertices 6 | 7 | Once the SE(3) for each warp node is computed, the warp field provides a warp function W(x) which transforms a point x in the canonical space to a point in the live frame. 8 | 9 | The Fusion object is initialized with a truncated signed distance volumne of the first frame (treated as the canonical frame). 10 | To process a new frame, we need two things: tsdf of the new frame and correponding points. The processing of a new frame is done by something like: 11 | 12 | fusion.solve(corr_to_current_frame) 13 | fusion.updateTSDF(tsdf_of_current_frame) 14 | fusion.update_graph() 15 | 16 | 17 | All input/output datatypes should be be numpy arrays. 18 | 19 | Important hyperparameters: 20 | - subsample_rate: determine the density of deformation nodes embedded in the canonical mesh 21 | - marching_cubes_step_size: determine the quality of the extracted mesh from marching cube 22 | - knn: affect blending and regularization. 23 | - wmax: maximum weight that can be accumulated in tsdf update. Affect the influences of the later fused frames. 24 | - method for finding correspondences 25 | 26 | TODO: 27 | - optimizer is too slow. Most time spent on Jacobian estimation 28 | - (Optional) provide an option to calculate dmax for DQB weight 29 | 30 | 31 | ''' 32 | 33 | import math 34 | import numpy as np 35 | import pickle 36 | import os 37 | from numpy import linalg as la 38 | import pyopencl as cl 39 | from scipy.spatial import KDTree 40 | from scipy.optimize import least_squares 41 | from scipy.sparse import lil_matrix 42 | from skimage import measure 43 | from .util import * 44 | from .sdf import * 45 | from . import * 46 | 47 | DATA_PATH = os.path.dirname(os.path.dirname(os.path.realpath(__file__))) 48 | 49 | 50 | # DATA_PATH = os.path.join(os.path.dirname(os.path.dirname(os.path.realpath(__file__))), 'data') 51 | 52 | 53 | class FusionDM: 54 | def __init__(self, trunc_distance, K, tsdf_res=256, subsample_rate=5.0, knn=4, marching_cubes_step_size=3, 55 | verbose=False, write_warpfield=True): 56 | 57 | self._itercounter = 0 58 | self._curr_tsdf = None 59 | self._tdist = abs(trunc_distance) 60 | self._tsdf_res = tsdf_res 61 | self._tsdf = np.zeros((tsdf_res, tsdf_res, tsdf_res)) + self._tdist 62 | self._tsdfw = np.zeros(self._tsdf.shape) 63 | self._lw = np.array([1, 0, 0, 0, 0, 0, 0, 0], dtype=np.float32) 64 | 65 | # intrinsic matrix K must be 3x3, non-singular 66 | self._K = K 67 | self._Kinv = la.inv(K) 68 | 69 | # coordinate mapping from XYZ indices to world coordinates 70 | self._IND = np.eye(4) 71 | self._INDinv = la.inv(self._IND) 72 | 73 | self._knn = knn 74 | self._marching_cubes_step_size = marching_cubes_step_size 75 | self._subsample_rate = subsample_rate 76 | self._nodes = [] 77 | self._neighbor_look_up = [] 78 | self._correspondences = [] 79 | self._kdtree = None 80 | self._verbose = verbose 81 | self._write_warpfield = write_warpfield 82 | 83 | ''' 84 | self.marching_cubes() 85 | average_distances = [] 86 | for f in self._faces: 87 | average_distances.append(self.average_edge_dist_in_face(f)) 88 | self._radius = subsample_rate * np.average(np.array(average_distances)) 89 | 90 | if verbose: 91 | print("Constructing initial graph...") 92 | self.construct_graph() 93 | ''' 94 | 95 | def compute_live_tsdf(self, depths, lws, UseAutoAlignment=False, useICP=False, outputMesh=False): 96 | if len(depths) != len(lws): 97 | raise ValueError('length of camera matrix array Ks must equal that of depth maps') 98 | 99 | tsdf_size = self._tsdf_res 100 | tsdf = np.zeros((tsdf_size, tsdf_size, tsdf_size)) + self._tdist 101 | tsdfw = np.zeros(tsdf.shape) 102 | 103 | avgs = [] 104 | stds = [] 105 | 106 | avg = np.array([-0.03, -0.43, -5.6], dtype='float32') 107 | std = 1.3 108 | 109 | # align data? 110 | if UseAutoAlignment: 111 | if self._verbose: 112 | print('estimating pt center and scale from input data') 113 | for idx in range(len(depths)): 114 | dm = depths[idx] 115 | dx, dy = dm.shape 116 | A = lws[idx] 117 | pts = [] 118 | for x in range(dx): 119 | for y in range(dy): 120 | if dm[x][y] != 0: 121 | A_inv = inverse_rigid_matrix(A) 122 | uv = -1 * dm[x][y] * np.array([y, x, 1], dtype='float') 123 | pos3 = np.matmul(self._Kinv, uv) 124 | pos3_can = np.matmul(A_inv, np.append(pos3, 1)) 125 | pts.append(pos3_can) 126 | 127 | pts = np.array(pts) 128 | avgs.append(np.average(pts, axis=0)) 129 | stds.append(np.std(pts, axis=0)) 130 | 131 | avgs = np.array(avgs) 132 | stds = np.array(stds) 133 | avg = np.average(avgs, axis=0) 134 | std = np.average(stds) 135 | 136 | scale = 8 * std / self._tsdf_res 137 | self._IND[0, 0] = scale 138 | self._IND[1, 1] = scale 139 | self._IND[2, 2] = scale 140 | self._IND[0:3, 3] = avg - scale * self._tsdf_res / 2 141 | self._INDinv = la.inv(self._IND) 142 | 143 | num_of_map = len(depths) 144 | 145 | if self._verbose: 146 | print('estimate center pt of input depth maps:', avg) 147 | print('estimate std of input depth maps:', std) 148 | 149 | if useICP: 150 | for idx in range(num_of_map): 151 | print('fusing depth map ', idx) 152 | tsdf = np.zeros((tsdf_size, tsdf_size, tsdf_size)) + self._tdist 153 | tsdfw = np.zeros(tsdf.shape) 154 | (tsdf, tsdfw) = self.fuseDepths(depths[idx], lws[idx], tsdf, tsdfw, scale=10 * std / self._tsdf_res, 155 | center=avg) 156 | if idx == 0: 157 | self._tsdf = tsdf 158 | self._tsdfw = tsdfw 159 | self.marching_cubes() 160 | else: 161 | # perform rigid icp alignment 162 | self._lw = np.array([1, 0, 0, 0, 0, 0, 0, 0], dtype=np.float32) 163 | self.solve(tsdf) 164 | self.updateTSDF(tsdf) 165 | else: 166 | for idx in range(num_of_map): 167 | self._depthidx = idx 168 | print('fusing depth map ', idx) 169 | (tsdf, tsdfw) = self.fuseDepths(depths[idx], lws[idx], tsdf, tsdfw, scale=12 * std / self._tsdf_res, 170 | center=avg) 171 | self._tsdf = tsdf 172 | self._tsdfw = tsdfw 173 | 174 | if outputMesh: 175 | np.save('tsdf_temp', self._tsdf) 176 | self.write_canonical_mesh(DATA_PATH, 'test.obj') 177 | 178 | return (tsdf, tsdfw) 179 | 180 | def fuseDepths(self, dm, lw, tsdf, tsdf_w, scale=1.0, center=np.zeros(3), wmax=100.0): 181 | (dmx, dmy) = dm.shape 182 | print('shape of depth map:', dm.shape) 183 | sdf_center = np.zeros(3) + self._tsdf_res / 2 184 | 185 | pt_count = 0 186 | it = np.nditer(tsdf, flags=['multi_index'], op_flags=['readwrite']) 187 | while not it.finished: 188 | pos = np.array(it.multi_index, dtype=np.float32) 189 | 190 | # align coordinate 191 | pos = scale * (pos - sdf_center) + center 192 | # pos = scale * (pos - sdf_center) 193 | lpos = np.matmul(lw, np.append(pos, 1)) 194 | (u, v) = project_to_pixel(self._K, lpos) 195 | if u is not None and v is not None and u >= 0 and u < dmy - 1 and v >= 0 and v < dmx - 1: 196 | z = -1 * dm[int(round(v))][int(round(u))] 197 | if z > 0: 198 | uc = z * np.array([u, v, 1]) 199 | # signed distance along principal axis of the camera 200 | cpos = np.matmul(self._Kinv, uc) 201 | tsdf_l = cpos[2] - lpos[2] 202 | # tsdf_l = np.sign(cpos[2] - lpos[2]) * la.norm(cpos - lpos) 203 | if tsdf_l > -1 * self._tdist: 204 | pt_count += 1 205 | # TODO: weight here may encode sensor uncertainty 206 | wi = 1 207 | wi_t = tsdf_w[it.multi_index] 208 | # Update (v(x),w(x)) 209 | it[0] = (scale * it[0] * wi_t + min(self._tdist, tsdf_l) * wi) / (scale * (wi + wi_t)) 210 | tsdf_w[it.multi_index] = min(wi + wi_t, wmax) 211 | 212 | it.iternext() 213 | 214 | if self._verbose: 215 | print("processedprojection pts: ", pt_count) 216 | print(tsdf.min()) 217 | return (tsdf, tsdf_w) 218 | 219 | def setupCorrespondences(self, curr_tsdf, prune_result=True, tolerance=1.0): 220 | self._correspondences = [] 221 | self._corridx = [] 222 | lverts, lfaces, lnormals, lvalues = self.marching_cubes(curr_tsdf, step_size=1) 223 | 224 | print('lverts shape:', lverts.shape) 225 | 226 | l_kdtree = KDTree(lverts) 227 | i = 0 228 | 229 | for idx in range(len(self._vertices)): 230 | wn = dqb_warp_normal(self._lw, self._normals[idx]) 231 | vp = dqb_warp(self._lw, self._vertices[idx]) 232 | dists, nidxs = l_kdtree.query(vp, k=self._knn) 233 | best_pt = lverts[nidxs[0]] 234 | best_cost = 1 235 | 236 | for nidx in nidxs: 237 | p = lverts[nidx] 238 | cost = abs(np.dot(wn, vp - p)) 239 | if cost < best_cost: 240 | best_cost = cost 241 | best_pt = p 242 | if best_cost <= tolerance: 243 | self._corridx.append(idx) 244 | self._correspondences.append(best_pt) 245 | 246 | ''' 247 | if prune_result: 248 | # update data (HACKHACK) 249 | self._vertices = np.delete(self._vertices, idx_pruned, axis=0) 250 | self._correspondences = np.delete(self._correspondences, idx_pruned, axis=0) 251 | self._neighbor_look_up = np.delete(self._neighbor_look_up, idx_pruned, axis=0) 252 | self._normals = np.delete(self._normals, idx_pruned, axis=0) 253 | self._faces = None 254 | 255 | vert_kdtree = KDTree(self._vertices) 256 | for i in range(len(self._nodes)): 257 | pos = self._nodes[i][1] 258 | se3 = self._nodes[i][2] 259 | dist, vidx = vert_kdtree.query(pos) 260 | self._nodes[i] = (vidx, pos, se3, 2*self._radius) 261 | ''' 262 | 263 | # Solve for a rigid transformation with correspondences to current tsdf 264 | def solve(self, curr_tsdf): 265 | iteration = 3 266 | self._itercounter += 1 267 | solver_verbose_level = 0 268 | if self._verbose: 269 | solver_verbose_level = 2 270 | 271 | if self._verbose: 272 | print('estimating global transformation lw...') 273 | 274 | for iter in range(iteration): 275 | self.setupCorrespondences(curr_tsdf) 276 | values = self._lw 277 | opt_result = least_squares(self.computef_lw, 278 | values, 279 | max_nfev=100, 280 | verbose=solver_verbose_level) 281 | print('global transformation found:', DQTSE3(opt_result.x)) 282 | self._lw = opt_result.x 283 | 284 | # x: current global transformation lw 285 | def computef_lw(self, x): 286 | f = [] 287 | # Data Term only 288 | i = 0 289 | for idx in self._corridx: 290 | wn = dqb_warp_normal(x, self._normals[idx]) 291 | vp = dqb_warp(x, self._vertices[idx]) 292 | corrp = self._correspondences[i] 293 | p2s = np.dot(wn, vp - corrp) 294 | # f.append(np.sign(p2s) * math.sqrt(tukey_biweight_loss(abs(p2s),tdw))) 295 | f.append(p2s) 296 | i += 1 297 | return np.array(f) 298 | 299 | # Perform surface fusion for each voxel center with a tsdf query function for the live frame. 300 | def updateTSDF(self, curr_tsdf, wmax=100.0): 301 | 302 | it = np.nditer(self._tsdf, flags=['multi_index'], op_flags=['readwrite']) 303 | while not it.finished: 304 | tsdf_s = np.copy(it[0]) 305 | pos = np.array(it.multi_index, dtype=np.float32) 306 | warped_pos = dqb_warp(self._lw, pos) 307 | tsdf_l = interpolate_tsdf(warped_pos, curr_tsdf) 308 | if tsdf_l is not None and tsdf_l > -1 * self._tdist: 309 | wi = 1 310 | wi_t = self._tsdfw[it.multi_index] 311 | it[0] = (it[0] * wi_t + min(self._tdist, tsdf_l) * wi) / (wi + wi_t) 312 | self._tsdfw[it.multi_index] = min(wi + wi_t, wmax) 313 | it.iternext() 314 | 315 | if self._verbose: 316 | print("Completed fusion of two tsdf") 317 | 318 | # Mesh vertices and normal extraction from current tsdf in canonical space 319 | def marching_cubes(self, tsdf=None, step_size=1): 320 | 321 | if step_size < 1: 322 | step_size = self._marching_cubes_step_size 323 | 324 | if tsdf is not None: 325 | return measure.marching_cubes_lewiner(tsdf, 326 | step_size=step_size) 327 | 328 | self._vertices, self._faces, self._normals, values = measure.marching_cubes_lewiner(self._tsdf, 329 | step_size=step_size) 330 | if self._verbose: 331 | print("Marching Cubes result: number of extracted vertices is %d" % (len(self._vertices))) 332 | 333 | # Write the current warp field to file 334 | def write_warp_field(self, path, filename): 335 | file = open(os.path.join(path, filename + '__' + str(self._itercounter) + '.p'), 'wb') 336 | pickle.dump(self._nodes, file) 337 | 338 | # Write the canonical mesh to file 339 | def write_canonical_mesh(self, path, filename): 340 | fpath = open(os.path.join(path, filename), 'w') 341 | verts, faces, normals, values = measure.marching_cubes_lewiner(self._tsdf, level=0, step_size=1, 342 | allow_degenerate=False) 343 | 344 | rot = self._IND[:3,:3] 345 | trans = self._IND[:3, 3] 346 | for v in verts: 347 | v = np.matmul(rot, v) + trans 348 | fpath.write('v %f %f %f\n' % (v[0], v[1], v[2])) 349 | for n in normals: 350 | n = np.matmul(rot, n) 351 | fpath.write('vn %f %f %f\n' % (n[0], n[1], n[2])) 352 | for f in faces: 353 | fpath.write('f %d//%d %d//%d %d//%d\n' % (f[0] + 1, f[0] + 1, f[1] + 1, f[1] + 1, f[2] + 1, f[2] + 1)) 354 | fpath.close() 355 | 356 | # Process a warp field file and write the live frame mesh 357 | def write_live_frame_mesh(self, path, filename, warpfield_path): 358 | pass 359 | 360 | def average_edge_dist_in_face(self, f): 361 | v1 = self._vertices[f[0]] 362 | v2 = self._vertices[f[1]] 363 | v3 = self._vertices[f[2]] 364 | return (cal_dist(v1, v2) + cal_dist(v1, v3) + cal_dist(v2, v3)) / 3 365 | 366 | # ----------------------------------------------------- functions below not useful for now ---------------------------------------------# 367 | 368 | # Optional: we can compute a sparsity structure to speed up the optimizer 369 | def computeSparsity(self, n, m): 370 | sparsity = lil_matrix((n, m), dtype=np.float32) 371 | ''' 372 | fill non-zero entries with 1 373 | ''' 374 | data_term_length = len(self._vertices) 375 | 376 | for idx in range(data_term_length): 377 | locations = self._neighbor_look_up[idx] 378 | for loc in locations: 379 | for i in range(8): 380 | sparsity[idx, 8 * loc + i] = 1 381 | 382 | for idx in range(len(self._nodes)): 383 | for i in range(8): 384 | sparsity[data_term_length + 3 * idx, 8 * (idx) + i] = 1 385 | sparsity[data_term_length + 3 * idx + 1, 8 * (idx) + i] = 1 386 | sparsity[data_term_length + 3 * idx + 2, 8 * (idx) + i] = 1 387 | 388 | for nidx in self._neighbor_look_up[self._nodes[idx][0]]: 389 | for i in range(8): 390 | sparsity[data_term_length + 3 * idx, 8 * (nidx) + i] = 1 391 | sparsity[data_term_length + 3 * idx + 1, 8 * (nidx) + i] = 1 392 | sparsity[data_term_length + 3 * idx + 2, 8 * (nidx) + i] = 1 393 | 394 | return sparsity 395 | 396 | # Compute residual function. Input is a flattened vector {dg_SE3} 397 | def computef(self, x, tdw, trw, rw): 398 | f = [] 399 | dqs = np.split(x, len(x) / 8) 400 | 401 | dte = 0 402 | rte = 0 403 | # Data Term 404 | for idx in range(len(self._vertices)): 405 | locations = self._neighbor_look_up[idx] 406 | knn_dqs = [dqs[i] for i in locations] 407 | vert_warped, n_warped = self.warp(self._vertices[idx], knn_dqs, locations, self._normals[idx], 408 | m_lw=self._lw) 409 | p2s = np.dot(n_warped, vert_warped - self._correspondences[idx]) 410 | f.append(p2s) 411 | # f.append(np.sign(p2s) * math.sqrt(tukey_biweight_loss(abs(p2s),tdw))) 412 | dte += f[-1] ** 2 413 | # Regularization Term: Instead of regularization tree, just use the simpler knn nodes for now 414 | for idx in range(len(self._nodes)): 415 | dgi_se3 = dqs[idx] 416 | for nidx in self._neighbor_look_up[self._nodes[idx][0]]: 417 | dgj_v = self._nodes[nidx][1] 418 | dgj_se3 = dqs[nidx] 419 | diff = dqb_warp(dgi_se3, dgj_v) - dqb_warp(dgj_se3, dgj_v) 420 | for i in range(3): 421 | f.append(rw * max(self._nodes[idx][3], self._nodes[nidx][3]) * diff[i]) 422 | # f.append(np.sign(diff[i]) * math.sqrt(rw * max(self._nodes[idx][3], self._nodes[nidx][3]) * huber_loss(diff[i], trw))) 423 | rte += f[-1] ** 2 424 | 425 | ''' 426 | if self._verbose: 427 | print("Data term energy:%f; reg term energy:%f"%(dte,rte)) 428 | ''' 429 | 430 | return np.array(f) 431 | 432 | ''' 433 | Warp a point from canonical space to the current live frame, using the wrap field computed from t-1. No camera matrix needed. 434 | params: 435 | dq: dual quaternions {dg_SE3} used for dual quaternion blending 436 | locations: indices for corresponding node in the graph. 437 | normal: if provided, return a warped normal as well. 438 | dmax: if provided, use the value instead of dgw to calculate weights 439 | m_lw: if provided, apply global rigid transformation 440 | ''' 441 | 442 | def warp(self, pos, dqs=None, locations=None, normal=None, dmax=None, m_lw=None): 443 | if dqs is None or locations is None: 444 | dists, kdidx = self._kdtree.query(pos, k=self._knn + 1) 445 | locations = kdidx[:-1] 446 | dqs = [self._nodes[i][2] for i in locations] 447 | 448 | se3 = self.dq_blend(pos, dqs, locations, dmax) 449 | 450 | pos_warped = dqb_warp(se3, pos) 451 | if m_lw is not None: 452 | pos_warped = dqb_warp(m_lw, pos_warped) 453 | 454 | if normal is not None: 455 | normal_warped = dqb_warp_normal(se3, normal) 456 | if m_lw is not None: 457 | normal_warped = dqb_warp_normal(m_lw, normal_warped) 458 | return (pos_warped, normal_warped) 459 | else: 460 | return pos_warped 461 | 462 | ''' 463 | Not sure how to determine dgw, so following idea from [Sumner 07] to calculate weights in DQB. 464 | The idea is to use the knn + 1 node as a denominator. 465 | ''' 466 | 467 | # Interpolate a se3 matrix from k-nearest nodes to pos. 468 | def dq_blend(self, pos, dqs=None, locations=None, dmax=None): 469 | if dqs is None or locations is None: 470 | dists, locations = self._kdtree.query(pos, k=self._knn) 471 | dqs = [self._nodes[i][2] for i in locations] 472 | 473 | dqb = np.zeros(8) 474 | for idx in range(len(dqs)): 475 | dg_idx, dg_v, dg_se3, dg_w = self._nodes[locations[idx]] 476 | dg_dq = dqs[idx] 477 | if dmax is None: 478 | w = math.exp(-1.0 * (la.norm(pos - dg_v) / (2 * dg_w)) ** 2) 479 | dqb += w * dg_dq 480 | else: 481 | w = math.exp(-1.0 * (la.norm(pos - dg_v) / dmax) ** 2) 482 | dqb += w * dg_dq 483 | 484 | # Hackhack 485 | if la.norm(dqb) == 0: 486 | if self._verbose: 487 | print('Really weird thing just happend!!!! blended dq is a zero vector') 488 | print('dqs:', dqs) 489 | print('locations:', locations) 490 | return np.array([1, 0, 0, 0, 0, 0, 0, 0], dtype=np.float32) 491 | 492 | return dqb / la.norm(dqb) 493 | 494 | # Construct deformation graph from canonical vertices 495 | def construct_graph(self): 496 | # uniform sampling 497 | nodes_v, nodes_idx = uniform_sample(self._vertices, self._radius) 498 | if self._verbose: 499 | print("%d deformation nodes sampled, with average radius of %f" % (len(nodes_v), self._radius)) 500 | 501 | ''' 502 | Each node is a 4-tuple (index of corresponding surface vertex dg_idx, 3D position dg_v, 4x4 Transformation dg_se3, weight dg_w) 503 | HackHack: 504 | Not sure how to determine dgw. Use sample radius for now. 505 | ''' 506 | for i in range(len(nodes_v)): 507 | self._nodes.append((nodes_idx[i], 508 | nodes_v[i], 509 | np.array([1, 0.00, 0.00, 0.00, 0.00, 0.01, 0.01, 0.00], dtype=np.float32), 510 | 2 * self._radius)) 511 | 512 | # construct kd tree 513 | self._kdtree = KDTree(nodes_v) 514 | self._neighbor_look_up = [] 515 | for vert in self._vertices: 516 | dists, idx = self._kdtree.query(vert, k=self._knn) 517 | self._neighbor_look_up.append(idx) 518 | 519 | # Update the deformation graph after new surafce vertices are found 520 | def update_graph(self): 521 | self.marching_cubes() 522 | # update topology of existing nodes 523 | vert_kdtree = KDTree(self._vertices) 524 | for i in range(len(self._nodes)): 525 | pos = self._nodes[i][1] 526 | se3 = self._nodes[i][2] 527 | dist, vidx = vert_kdtree.query(pos) 528 | self._nodes[i] = (vidx, pos, se3, 2 * self._radius) 529 | 530 | # find unsupported surface points 531 | unsupported_vert = [] 532 | for vert in self._vertices: 533 | dists, kdidx = self._kdtree.query(vert, k=self._knn) 534 | if min([la.norm(self._nodes[idx][1] - vert) / self._nodes[idx][3] for idx in kdidx]) >= 1: 535 | unsupported_vert.append(vert) 536 | 537 | nodes_new_v, nodes_new_idx = uniform_sample(unsupported_vert, self._radius) 538 | for i in range(len(nodes_new_v)): 539 | self._nodes.append((nodes_new_idx[i], 540 | nodes_new_v[i], 541 | self.dq_blend(nodes_new_v[i]), 542 | 2 * self._radius)) 543 | 544 | 545 | if self._verbose: 546 | print("Inserted %d new deformation nodes. Current number of deformation nodes: %d" % ( 547 | len(nodes_new_v), len(self._nodes))) 548 | 549 | # recompute KDTree and neighbors 550 | self._kdtree = KDTree(np.array([n[1] for n in self._nodes])) 551 | self._neighbor_look_up = [] 552 | for vert in self._vertices: 553 | dists, idx = self._kdtree.query(vert, k=self._knn) 554 | self._neighbor_look_up.append(idx) 555 | 556 | # since fusion is complete at this point, delete current live frame data 557 | self._curr_tsdf = None 558 | self._correspondences = [] 559 | if self._write_warpfield: 560 | self.write_warp_field(DATA_PATH, 'test') 561 | 562 | 563 | class FusionDM_GPU(FusionDM): 564 | def __init__(self, trunc_distance, K, tsdf_res=256, subsample_rate=5.0, knn=4, marching_cubes_step_size=3, 565 | verbose=False, write_warpfield=True): 566 | FusionDM.__init__(self, trunc_distance=trunc_distance, 567 | K=K, tsdf_res=tsdf_res, subsample_rate=subsample_rate, 568 | knn=knn, marching_cubes_step_size=marching_cubes_step_size, 569 | verbose=verbose, 570 | write_warpfield=write_warpfield) 571 | self._cl_ctx = cl.create_some_context() 572 | self._cl_queue = cl.CommandQueue(self._cl_ctx) 573 | if verbose: 574 | self.verbose_gpu() 575 | 576 | def verbose_gpu(self): 577 | print('\n' + '=' * 60 + '\nOpenCL Platforms and Devices') 578 | # Print each platform on this computer 579 | for platform in cl.get_platforms(): 580 | print('=' * 60) 581 | print('Platform - Name: ' + platform.name) 582 | print('Platform - Vendor: ' + platform.vendor) 583 | print('Platform - Version: ' + platform.version) 584 | print('Platform - Profile: ' + platform.profile) 585 | # Print each device per-platform 586 | for device in platform.get_devices(): 587 | print(' ' + '-' * 56) 588 | print(' Device - Name: ' + device.name) 589 | print(' Device - Type: ' + cl.device_type.to_string(device.type)) 590 | print(' Device - Max Clock Speed: {0} Mhz'.format(device.max_clock_frequency)) 591 | print(' Device - Compute Units: {0}'.format(device.max_compute_units)) 592 | print(' Device - Local Memory: {0:.0f} KB'.format(device.local_mem_size / 1024.0)) 593 | print(' Device - Constant Memory: {0:.0f} KB'.format(device.max_constant_buffer_size / 1024.0)) 594 | print(' Device - Global Memory: {0:.0f} GB'.format(device.global_mem_size / 1073741824.0)) 595 | print( 596 | ' Device - Max Buffer/Image Size: {0:.0f} MB'.format(device.max_mem_alloc_size / 1048576.0)) 597 | print(' Device - Max Work Group Size: {0:.0f}'.format(device.max_work_group_size)) 598 | print('\n') 599 | 600 | def fuseDepths(self, dm, lw, tsdf, tsdf_w, scale=1.0, center=np.zeros(3), wmax=100.0): 601 | (dmy, dmx) = dm.shape 602 | print('shape of depth map:', dm.shape) 603 | sdf_center = np.zeros(3) + self._tsdf_res / 2 604 | kernel = r""" 605 | inline float interpolation(__global const float *depth, float px, float py) 606 | { 607 | int x = floor(px); 608 | int y = floor(py); 609 | float wx = px - x; 610 | float wy = py - y; 611 | 612 | int left_up_id = y * DM_X + x; 613 | int left_bot_id = (y+1) * DM_X + x; 614 | int right_up_id = left_up_id + 1; 615 | int right_bot_id = left_bot_id + 1; 616 | 617 | float up_depth = depth[left_up_id] * (1 - wx) + depth[right_up_id] * wx; 618 | float bot_depth = depth[left_bot_id] * (1 - wx) + depth[right_bot_id] * wx; 619 | float ret = up_depth * (1 - wy) + bot_depth * wy; 620 | 621 | return ret; 622 | } 623 | 624 | inline float naive(__global const float *depth, float px, float py) 625 | { 626 | int id = round(py) * DM_X + round(px); 627 | return depth[id]; 628 | } 629 | 630 | __kernel void fuse_depth(__global float *tsdf, __global float *tsdf_w, __global const float *depth, __global const float *proj, __global const float *K_inv) 631 | { 632 | int x = get_global_id(0); 633 | int y = get_global_id(1); 634 | int z = get_global_id(2); 635 | 636 | // index in the tsdf array 637 | int idx = x * RES_Z * RES_Y + y * RES_Z + z; 638 | 639 | // project to the image space 640 | float u = proj[0] * x + proj[1] * y + proj[2] * z + proj[3]; 641 | float v = proj[4] * x + proj[5] * y + proj[6] * z + proj[7]; 642 | float w = proj[8] * x + proj[9] * y + proj[10] * z + proj[11]; 643 | 644 | // depth location 645 | float px = u / w; 646 | float py = v / w; 647 | if (px < 0 || py < 0 || px >= DM_X - 1 || py >= DM_Y - 1) 648 | return; 649 | float pz = -interpolation(depth, px, py); 650 | 651 | float dz; 652 | if (pz <= TDIST) 653 | dz = -TDIST; 654 | else { 655 | px *= pz; 656 | py *= pz; 657 | dz = K_inv[6] * (px - u) + K_inv[7] * (py - v) + K_inv[8] * (pz - w); 658 | dz = -dz; 659 | } 660 | float old_tsdf = tsdf[idx]; 661 | /* 662 | if (old_tsdf > 0) 663 | tsdf[idx] = min(old_tsdf, dz); 664 | else if (dz <= 0) 665 | tsdf[idx] = max(old_tsdf, dz); 666 | */ 667 | if (dz < TDIST) 668 | { 669 | float w = 1; 670 | float new_w = min(w + tsdf_w[idx], WMAX); 671 | tsdf[idx] = ((new_w - w) * old_tsdf + w * max(-TDIST, dz)) / new_w; 672 | tsdf_w[idx] = new_w; 673 | } 674 | }""" 675 | 676 | program = cl.Program(self._cl_ctx, """ 677 | #define DM_X %d 678 | #define DM_Y %d 679 | #define RES_X %d 680 | #define RES_Y %d 681 | #define RES_Z %d 682 | #define TDIST %ff 683 | #define WMAX %ff 684 | """ % (dmx, dmy, 685 | tsdf.shape[0], tsdf.shape[1], tsdf.shape[2], 686 | self._tdist, wmax 687 | ) + kernel).build() 688 | 689 | mf = cl.mem_flags 690 | tsdf = tsdf.astype(np.float32) 691 | tsdf_w = tsdf_w.astype(np.float32) 692 | 693 | tsdf_buffer = cl.Buffer(self._cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=tsdf) 694 | tsdf_w_buffer = cl.Buffer(self._cl_ctx, mf.READ_WRITE | mf.COPY_HOST_PTR, hostbuf=tsdf_w) 695 | proj = np.matmul(self._K, np.matmul(lw, self._IND)) 696 | proj_buffer = cl.Buffer(self._cl_ctx, mf.READ_ONLY | mf.COPY_HOST_PTR, hostbuf=proj.astype(np.float32)) 697 | Kinv_buffer = cl.Buffer(self._cl_ctx, mf.READ_ONLY | mf.COPY_HOST_PTR, hostbuf=self._Kinv.astype(np.float32)) 698 | depth_buffer = cl.Buffer(self._cl_ctx, mf.READ_ONLY | mf.COPY_HOST_PTR, hostbuf=dm.astype(np.float32)) 699 | 700 | program.fuse_depth(self._cl_queue, tsdf.shape, None, tsdf_buffer, tsdf_w_buffer, depth_buffer, proj_buffer, Kinv_buffer) 701 | cl.enqueue_read_buffer(self._cl_queue, tsdf_buffer, tsdf) 702 | cl.enqueue_read_buffer(self._cl_queue, tsdf_w_buffer, tsdf_w) 703 | self._cl_queue.finish() 704 | pt_count = 0 705 | if False: 706 | it = np.nditer(tsdf, flags=['multi_index'], op_flags=['readwrite']) 707 | while not it.finished: 708 | pos = np.array(it.multi_index, dtype=np.float32) 709 | 710 | # align coordinate 711 | pos = np.matmul(self._IND, np.append(pos, 1)) 712 | # pos = scale * (pos - sdf_center) 713 | lpos = np.matmul(lw, pos) 714 | (u, v) = project_to_pixel(self._K, lpos) 715 | if u is not None and v is not None and u >= 0 and u < dmy - 1 and v >= 0 and v < dmx - 1: 716 | z = -1 * dm[int(round(v))][int(round(u))] 717 | if z > 0: 718 | uc = z * np.array([u, v, 1]) 719 | # signed distance along principal axis of the camera 720 | cpos = np.matmul(self._Kinv, uc) 721 | tsdf_l = cpos[2] - lpos[2] 722 | # tsdf_l = np.sign(cpos[2] - lpos[2]) * la.norm(cpos - lpos) 723 | if tsdf_l > -1 * self._tdist: 724 | pt_count += 1 725 | # TODO: weight here may encode sensor uncertainty 726 | wi = 1 727 | wi_t = tsdf_w[it.multi_index] 728 | # Update (v(x),w(x)) 729 | it[0] = (scale * it[0] * wi_t + min(self._tdist, tsdf_l) * wi) / (scale * (wi + wi_t)) 730 | tsdf_w[it.multi_index] = min(wi + wi_t, wmax) 731 | 732 | it.iternext() 733 | 734 | if self._verbose: 735 | print("processedprojection pts: ", pt_count) 736 | print(tsdf.min(), tsdf.max()) 737 | return (tsdf, tsdf_w) 738 | -------------------------------------------------------------------------------- /core/transformation.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | # -*- coding: utf-8 -*- 4 | # transformations.py 5 | 6 | # Copyright (c) 2006-2018, Christoph Gohlke 7 | # Copyright (c) 2006-2018, The Regents of the University of California 8 | # Produced at the Laboratory for Fluorescence Dynamics 9 | # All rights reserved. 10 | # 11 | # Redistribution and use in source and binary forms, with or without 12 | # modification, are permitted provided that the following conditions are met: 13 | # 14 | # * Redistributions of source code must retain the above copyright 15 | # notice, this list of conditions and the following disclaimer. 16 | # * Redistributions in binary form must reproduce the above copyright 17 | # notice, this list of conditions and the following disclaimer in the 18 | # documentation and/or other materials provided with the distribution. 19 | # * Neither the name of the copyright holders nor the names of any 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | """Homogeneous Transformation Matrices and Quaternions. 36 | 37 | A library for calculating 4x4 matrices for translating, rotating, reflecting, 38 | scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 39 | 3D homogeneous coordinates as well as for converting between rotation matrices, 40 | Euler angles, and quaternions. Also includes an Arcball control object and 41 | functions to decompose transformation matrices. 42 | 43 | :Author: 44 | `Christoph Gohlke `_ 45 | 46 | :Organization: 47 | Laboratory for Fluorescence Dynamics, University of California, Irvine 48 | 49 | :Version: 2018.02.08 50 | 51 | Requirements 52 | ------------ 53 | * `CPython 2.7 or 3.6 `_ 54 | * `Numpy 1.13 `_ 55 | * `Transformations.c 2018.02.08 `_ 56 | (recommended for speedup of some functions) 57 | 58 | Notes 59 | ----- 60 | The API is not stable yet and is expected to change between revisions. 61 | 62 | This Python code is not optimized for speed. Refer to the transformations.c 63 | module for a faster implementation of some functions. 64 | 65 | Documentation in HTML format can be generated with epydoc. 66 | 67 | Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using 68 | numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using 69 | numpy.dot(M, v) for shape (4, \*) column vectors, respectively 70 | numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points"). 71 | 72 | This module follows the "column vectors on the right" and "row major storage" 73 | (C contiguous) conventions. The translation components are in the right column 74 | of the transformation matrix, i.e. M[:3, 3]. 75 | The transpose of the transformation matrices may have to be used to interface 76 | with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16]. 77 | 78 | Calculations are carried out with numpy.float64 precision. 79 | 80 | Vector, point, quaternion, and matrix function arguments are expected to be 81 | "array like", i.e. tuple, list, or numpy arrays. 82 | 83 | Return types are numpy arrays unless specified otherwise. 84 | 85 | Angles are in radians unless specified otherwise. 86 | 87 | Quaternions w+ix+jy+kz are represented as [w, x, y, z]. 88 | 89 | A triple of Euler angles can be applied/interpreted in 24 ways, which can 90 | be specified using a 4 character string or encoded 4-tuple: 91 | 92 | *Axes 4-string*: e.g. 'sxyz' or 'ryxy' 93 | 94 | - first character : rotations are applied to 's'tatic or 'r'otating frame 95 | - remaining characters : successive rotation axis 'x', 'y', or 'z' 96 | 97 | *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) 98 | 99 | - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. 100 | - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed 101 | by 'z', or 'z' is followed by 'x'. Otherwise odd (1). 102 | - repetition : first and last axis are same (1) or different (0). 103 | - frame : rotations are applied to static (0) or rotating (1) frame. 104 | 105 | Other Python packages and modules for 3D transformations and quaternions: 106 | 107 | * `Transforms3d `_ 108 | includes most code of this module. 109 | * `Blender.mathutils `_ 110 | * `numpy-dtypes `_ 111 | 112 | References 113 | ---------- 114 | (1) Matrices and transformations. Ronald Goldman. 115 | In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. 116 | (2) More matrices and transformations: shear and pseudo-perspective. 117 | Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 118 | (3) Decomposing a matrix into simple transformations. Spencer Thomas. 119 | In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 120 | (4) Recovering the data from the transformation matrix. Ronald Goldman. 121 | In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. 122 | (5) Euler angle conversion. Ken Shoemake. 123 | In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. 124 | (6) Arcball rotation control. Ken Shoemake. 125 | In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. 126 | (7) Representing attitude: Euler angles, unit quaternions, and rotation 127 | vectors. James Diebel. 2006. 128 | (8) A discussion of the solution for the best rotation to relate two sets 129 | of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. 130 | (9) Closed-form solution of absolute orientation using unit quaternions. 131 | BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. 132 | (10) Quaternions. Ken Shoemake. 133 | http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf 134 | (11) From quaternion to matrix and back. JMP van Waveren. 2005. 135 | http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm 136 | (12) Uniform random rotations. Ken Shoemake. 137 | In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. 138 | (13) Quaternion in molecular modeling. CFF Karney. 139 | J Mol Graph Mod, 25(5):595-604 140 | (14) New method for extracting the quaternion from a rotation matrix. 141 | Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. 142 | (15) Multiple View Geometry in Computer Vision. Hartley and Zissermann. 143 | Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. 144 | (16) Column Vectors vs. Row Vectors. 145 | http://steve.hollasch.net/cgindex/math/matrix/column-vec.html 146 | 147 | Examples 148 | -------- 149 | >>> alpha, beta, gamma = 0.123, -1.234, 2.345 150 | >>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] 151 | >>> I = identity_matrix() 152 | >>> Rx = rotation_matrix(alpha, xaxis) 153 | >>> Ry = rotation_matrix(beta, yaxis) 154 | >>> Rz = rotation_matrix(gamma, zaxis) 155 | >>> R = concatenate_matrices(Rx, Ry, Rz) 156 | >>> euler = euler_from_matrix(R, 'rxyz') 157 | >>> numpy.allclose([alpha, beta, gamma], euler) 158 | True 159 | >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') 160 | >>> is_same_transform(R, Re) 161 | True 162 | >>> al, be, ga = euler_from_matrix(Re, 'rxyz') 163 | >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 164 | True 165 | >>> qx = quaternion_about_axis(alpha, xaxis) 166 | >>> qy = quaternion_about_axis(beta, yaxis) 167 | >>> qz = quaternion_about_axis(gamma, zaxis) 168 | >>> q = quaternion_multiply(qx, qy) 169 | >>> q = quaternion_multiply(q, qz) 170 | >>> Rq = quaternion_matrix(q) 171 | >>> is_same_transform(R, Rq) 172 | True 173 | >>> S = scale_matrix(1.23, origin) 174 | >>> T = translation_matrix([1, 2, 3]) 175 | >>> Z = shear_matrix(beta, xaxis, origin, zaxis) 176 | >>> R = random_rotation_matrix(numpy.random.rand(3)) 177 | >>> M = concatenate_matrices(T, R, Z, S) 178 | >>> scale, shear, angles, trans, persp = decompose_matrix(M) 179 | >>> numpy.allclose(scale, 1.23) 180 | True 181 | >>> numpy.allclose(trans, [1, 2, 3]) 182 | True 183 | >>> numpy.allclose(shear, [0, math.tan(beta), 0]) 184 | True 185 | >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 186 | True 187 | >>> M1 = compose_matrix(scale, shear, angles, trans, persp) 188 | >>> is_same_transform(M, M1) 189 | True 190 | >>> v0, v1 = random_vector(3), random_vector(3) 191 | >>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) 192 | >>> v2 = numpy.dot(v0, M[:3,:3].T) 193 | >>> numpy.allclose(unit_vector(v1), unit_vector(v2)) 194 | True 195 | 196 | """ 197 | 198 | from __future__ import division, print_function 199 | 200 | import math 201 | 202 | import numpy 203 | 204 | __version__ = '2018.02.08' 205 | __docformat__ = 'restructuredtext en' 206 | __all__ = () 207 | 208 | 209 | def identity_matrix(): 210 | """Return 4x4 identity/unit matrix. 211 | 212 | >>> I = identity_matrix() 213 | >>> numpy.allclose(I, numpy.dot(I, I)) 214 | True 215 | >>> numpy.sum(I), numpy.trace(I) 216 | (4.0, 4.0) 217 | >>> numpy.allclose(I, numpy.identity(4)) 218 | True 219 | 220 | """ 221 | return numpy.identity(4) 222 | 223 | 224 | def translation_matrix(direction): 225 | """Return matrix to translate by direction vector. 226 | 227 | >>> v = numpy.random.random(3) - 0.5 228 | >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) 229 | True 230 | 231 | """ 232 | M = numpy.identity(4) 233 | M[:3, 3] = direction[:3] 234 | return M 235 | 236 | 237 | def translation_from_matrix(matrix): 238 | """Return translation vector from translation matrix. 239 | 240 | >>> v0 = numpy.random.random(3) - 0.5 241 | >>> v1 = translation_from_matrix(translation_matrix(v0)) 242 | >>> numpy.allclose(v0, v1) 243 | True 244 | 245 | """ 246 | return numpy.array(matrix, copy=False)[:3, 3].copy() 247 | 248 | 249 | def reflection_matrix(point, normal): 250 | """Return matrix to mirror at plane defined by point and normal vector. 251 | 252 | >>> v0 = numpy.random.random(4) - 0.5 253 | >>> v0[3] = 1. 254 | >>> v1 = numpy.random.random(3) - 0.5 255 | >>> R = reflection_matrix(v0, v1) 256 | >>> numpy.allclose(2, numpy.trace(R)) 257 | True 258 | >>> numpy.allclose(v0, numpy.dot(R, v0)) 259 | True 260 | >>> v2 = v0.copy() 261 | >>> v2[:3] += v1 262 | >>> v3 = v0.copy() 263 | >>> v2[:3] -= v1 264 | >>> numpy.allclose(v2, numpy.dot(R, v3)) 265 | True 266 | 267 | """ 268 | normal = unit_vector(normal[:3]) 269 | M = numpy.identity(4) 270 | M[:3, :3] -= 2.0 * numpy.outer(normal, normal) 271 | M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal 272 | return M 273 | 274 | 275 | def reflection_from_matrix(matrix): 276 | """Return mirror plane point and normal vector from reflection matrix. 277 | 278 | >>> v0 = numpy.random.random(3) - 0.5 279 | >>> v1 = numpy.random.random(3) - 0.5 280 | >>> M0 = reflection_matrix(v0, v1) 281 | >>> point, normal = reflection_from_matrix(M0) 282 | >>> M1 = reflection_matrix(point, normal) 283 | >>> is_same_transform(M0, M1) 284 | True 285 | 286 | """ 287 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 288 | # normal: unit eigenvector corresponding to eigenvalue -1 289 | w, V = numpy.linalg.eig(M[:3, :3]) 290 | i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] 291 | if not len(i): 292 | raise ValueError('no unit eigenvector corresponding to eigenvalue -1') 293 | normal = numpy.real(V[:, i[0]]).squeeze() 294 | # point: any unit eigenvector corresponding to eigenvalue 1 295 | w, V = numpy.linalg.eig(M) 296 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 297 | if not len(i): 298 | raise ValueError('no unit eigenvector corresponding to eigenvalue 1') 299 | point = numpy.real(V[:, i[-1]]).squeeze() 300 | point /= point[3] 301 | return point, normal 302 | 303 | 304 | def rotation_matrix(angle, direction, point=None): 305 | """Return matrix to rotate about axis defined by point and direction. 306 | 307 | >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0]) 308 | >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) 309 | True 310 | >>> angle = (random.random() - 0.5) * (2*math.pi) 311 | >>> direc = numpy.random.random(3) - 0.5 312 | >>> point = numpy.random.random(3) - 0.5 313 | >>> R0 = rotation_matrix(angle, direc, point) 314 | >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) 315 | >>> is_same_transform(R0, R1) 316 | True 317 | >>> R0 = rotation_matrix(angle, direc, point) 318 | >>> R1 = rotation_matrix(-angle, -direc, point) 319 | >>> is_same_transform(R0, R1) 320 | True 321 | >>> I = numpy.identity(4, numpy.float64) 322 | >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) 323 | True 324 | >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2, 325 | ... direc, point))) 326 | True 327 | 328 | """ 329 | sina = math.sin(angle) 330 | cosa = math.cos(angle) 331 | direction = unit_vector(direction[:3]) 332 | # rotation matrix around unit vector 333 | R = numpy.diag([cosa, cosa, cosa]) 334 | R += numpy.outer(direction, direction) * (1.0 - cosa) 335 | direction *= sina 336 | R += numpy.array([[ 0.0, -direction[2], direction[1]], 337 | [ direction[2], 0.0, -direction[0]], 338 | [-direction[1], direction[0], 0.0]]) 339 | M = numpy.identity(4) 340 | M[:3, :3] = R 341 | if point is not None: 342 | # rotation not around origin 343 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 344 | M[:3, 3] = point - numpy.dot(R, point) 345 | return M 346 | 347 | 348 | def rotation_from_matrix(matrix): 349 | """Return rotation angle and axis from rotation matrix. 350 | 351 | >>> angle = (random.random() - 0.5) * (2*math.pi) 352 | >>> direc = numpy.random.random(3) - 0.5 353 | >>> point = numpy.random.random(3) - 0.5 354 | >>> R0 = rotation_matrix(angle, direc, point) 355 | >>> angle, direc, point = rotation_from_matrix(R0) 356 | >>> R1 = rotation_matrix(angle, direc, point) 357 | >>> is_same_transform(R0, R1) 358 | True 359 | 360 | """ 361 | R = numpy.array(matrix, dtype=numpy.float64, copy=False) 362 | R33 = R[:3, :3] 363 | # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 364 | w, W = numpy.linalg.eig(R33.T) 365 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 366 | if not len(i): 367 | raise ValueError('no unit eigenvector corresponding to eigenvalue 1') 368 | direction = numpy.real(W[:, i[-1]]).squeeze() 369 | # point: unit eigenvector of R33 corresponding to eigenvalue of 1 370 | w, Q = numpy.linalg.eig(R) 371 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 372 | if not len(i): 373 | raise ValueError('no unit eigenvector corresponding to eigenvalue 1') 374 | point = numpy.real(Q[:, i[-1]]).squeeze() 375 | point /= point[3] 376 | # rotation angle depending on direction 377 | cosa = (numpy.trace(R33) - 1.0) / 2.0 378 | if abs(direction[2]) > 1e-8: 379 | sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] 380 | elif abs(direction[1]) > 1e-8: 381 | sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] 382 | else: 383 | sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] 384 | angle = math.atan2(sina, cosa) 385 | return angle, direction, point 386 | 387 | 388 | def scale_matrix(factor, origin=None, direction=None): 389 | """Return matrix to scale by factor around origin in direction. 390 | 391 | Use factor -1 for point symmetry. 392 | 393 | >>> v = (numpy.random.rand(4, 5) - 0.5) * 20 394 | >>> v[3] = 1 395 | >>> S = scale_matrix(-1.234) 396 | >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) 397 | True 398 | >>> factor = random.random() * 10 - 5 399 | >>> origin = numpy.random.random(3) - 0.5 400 | >>> direct = numpy.random.random(3) - 0.5 401 | >>> S = scale_matrix(factor, origin) 402 | >>> S = scale_matrix(factor, origin, direct) 403 | 404 | """ 405 | if direction is None: 406 | # uniform scaling 407 | M = numpy.diag([factor, factor, factor, 1.0]) 408 | if origin is not None: 409 | M[:3, 3] = origin[:3] 410 | M[:3, 3] *= 1.0 - factor 411 | else: 412 | # nonuniform scaling 413 | direction = unit_vector(direction[:3]) 414 | factor = 1.0 - factor 415 | M = numpy.identity(4) 416 | M[:3, :3] -= factor * numpy.outer(direction, direction) 417 | if origin is not None: 418 | M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction 419 | return M 420 | 421 | 422 | def scale_from_matrix(matrix): 423 | """Return scaling factor, origin and direction from scaling matrix. 424 | 425 | >>> factor = random.random() * 10 - 5 426 | >>> origin = numpy.random.random(3) - 0.5 427 | >>> direct = numpy.random.random(3) - 0.5 428 | >>> S0 = scale_matrix(factor, origin) 429 | >>> factor, origin, direction = scale_from_matrix(S0) 430 | >>> S1 = scale_matrix(factor, origin, direction) 431 | >>> is_same_transform(S0, S1) 432 | True 433 | >>> S0 = scale_matrix(factor, origin, direct) 434 | >>> factor, origin, direction = scale_from_matrix(S0) 435 | >>> S1 = scale_matrix(factor, origin, direction) 436 | >>> is_same_transform(S0, S1) 437 | True 438 | 439 | """ 440 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 441 | M33 = M[:3, :3] 442 | factor = numpy.trace(M33) - 2.0 443 | try: 444 | # direction: unit eigenvector corresponding to eigenvalue factor 445 | w, V = numpy.linalg.eig(M33) 446 | i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] 447 | direction = numpy.real(V[:, i]).squeeze() 448 | direction /= vector_norm(direction) 449 | except IndexError: 450 | # uniform scaling 451 | factor = (factor + 2.0) / 3.0 452 | direction = None 453 | # origin: any eigenvector corresponding to eigenvalue 1 454 | w, V = numpy.linalg.eig(M) 455 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 456 | if not len(i): 457 | raise ValueError('no eigenvector corresponding to eigenvalue 1') 458 | origin = numpy.real(V[:, i[-1]]).squeeze() 459 | origin /= origin[3] 460 | return factor, origin, direction 461 | 462 | 463 | def projection_matrix(point, normal, direction=None, 464 | perspective=None, pseudo=False): 465 | """Return matrix to project onto plane defined by point and normal. 466 | 467 | Using either perspective point, projection direction, or none of both. 468 | 469 | If pseudo is True, perspective projections will preserve relative depth 470 | such that Perspective = dot(Orthogonal, PseudoPerspective). 471 | 472 | >>> P = projection_matrix([0, 0, 0], [1, 0, 0]) 473 | >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 474 | True 475 | >>> point = numpy.random.random(3) - 0.5 476 | >>> normal = numpy.random.random(3) - 0.5 477 | >>> direct = numpy.random.random(3) - 0.5 478 | >>> persp = numpy.random.random(3) - 0.5 479 | >>> P0 = projection_matrix(point, normal) 480 | >>> P1 = projection_matrix(point, normal, direction=direct) 481 | >>> P2 = projection_matrix(point, normal, perspective=persp) 482 | >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 483 | >>> is_same_transform(P2, numpy.dot(P0, P3)) 484 | True 485 | >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) 486 | >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 487 | >>> v0[3] = 1 488 | >>> v1 = numpy.dot(P, v0) 489 | >>> numpy.allclose(v1[1], v0[1]) 490 | True 491 | >>> numpy.allclose(v1[0], 3-v1[1]) 492 | True 493 | 494 | """ 495 | M = numpy.identity(4) 496 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 497 | normal = unit_vector(normal[:3]) 498 | if perspective is not None: 499 | # perspective projection 500 | perspective = numpy.array(perspective[:3], dtype=numpy.float64, 501 | copy=False) 502 | M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) 503 | M[:3, :3] -= numpy.outer(perspective, normal) 504 | if pseudo: 505 | # preserve relative depth 506 | M[:3, :3] -= numpy.outer(normal, normal) 507 | M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) 508 | else: 509 | M[:3, 3] = numpy.dot(point, normal) * perspective 510 | M[3, :3] = -normal 511 | M[3, 3] = numpy.dot(perspective, normal) 512 | elif direction is not None: 513 | # parallel projection 514 | direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) 515 | scale = numpy.dot(direction, normal) 516 | M[:3, :3] -= numpy.outer(direction, normal) / scale 517 | M[:3, 3] = direction * (numpy.dot(point, normal) / scale) 518 | else: 519 | # orthogonal projection 520 | M[:3, :3] -= numpy.outer(normal, normal) 521 | M[:3, 3] = numpy.dot(point, normal) * normal 522 | return M 523 | 524 | 525 | def projection_from_matrix(matrix, pseudo=False): 526 | """Return projection plane and perspective point from projection matrix. 527 | 528 | Return values are same as arguments for projection_matrix function: 529 | point, normal, direction, perspective, and pseudo. 530 | 531 | >>> point = numpy.random.random(3) - 0.5 532 | >>> normal = numpy.random.random(3) - 0.5 533 | >>> direct = numpy.random.random(3) - 0.5 534 | >>> persp = numpy.random.random(3) - 0.5 535 | >>> P0 = projection_matrix(point, normal) 536 | >>> result = projection_from_matrix(P0) 537 | >>> P1 = projection_matrix(*result) 538 | >>> is_same_transform(P0, P1) 539 | True 540 | >>> P0 = projection_matrix(point, normal, direct) 541 | >>> result = projection_from_matrix(P0) 542 | >>> P1 = projection_matrix(*result) 543 | >>> is_same_transform(P0, P1) 544 | True 545 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 546 | >>> result = projection_from_matrix(P0, pseudo=False) 547 | >>> P1 = projection_matrix(*result) 548 | >>> is_same_transform(P0, P1) 549 | True 550 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 551 | >>> result = projection_from_matrix(P0, pseudo=True) 552 | >>> P1 = projection_matrix(*result) 553 | >>> is_same_transform(P0, P1) 554 | True 555 | 556 | """ 557 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 558 | M33 = M[:3, :3] 559 | w, V = numpy.linalg.eig(M) 560 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 561 | if not pseudo and len(i): 562 | # point: any eigenvector corresponding to eigenvalue 1 563 | point = numpy.real(V[:, i[-1]]).squeeze() 564 | point /= point[3] 565 | # direction: unit eigenvector corresponding to eigenvalue 0 566 | w, V = numpy.linalg.eig(M33) 567 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 568 | if not len(i): 569 | raise ValueError('no eigenvector corresponding to eigenvalue 0') 570 | direction = numpy.real(V[:, i[0]]).squeeze() 571 | direction /= vector_norm(direction) 572 | # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 573 | w, V = numpy.linalg.eig(M33.T) 574 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 575 | if len(i): 576 | # parallel projection 577 | normal = numpy.real(V[:, i[0]]).squeeze() 578 | normal /= vector_norm(normal) 579 | return point, normal, direction, None, False 580 | else: 581 | # orthogonal projection, where normal equals direction vector 582 | return point, direction, None, None, False 583 | else: 584 | # perspective projection 585 | i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] 586 | if not len(i): 587 | raise ValueError( 588 | 'no eigenvector not corresponding to eigenvalue 0') 589 | point = numpy.real(V[:, i[-1]]).squeeze() 590 | point /= point[3] 591 | normal = - M[3, :3] 592 | perspective = M[:3, 3] / numpy.dot(point[:3], normal) 593 | if pseudo: 594 | perspective -= normal 595 | return point, normal, None, perspective, pseudo 596 | 597 | 598 | def clip_matrix(left, right, bottom, top, near, far, perspective=False): 599 | """Return matrix to obtain normalized device coordinates from frustum. 600 | 601 | The frustum bounds are axis-aligned along x (left, right), 602 | y (bottom, top) and z (near, far). 603 | 604 | Normalized device coordinates are in range [-1, 1] if coordinates are 605 | inside the frustum. 606 | 607 | If perspective is True the frustum is a truncated pyramid with the 608 | perspective point at origin and direction along z axis, otherwise an 609 | orthographic canonical view volume (a box). 610 | 611 | Homogeneous coordinates transformed by the perspective clip matrix 612 | need to be dehomogenized (divided by w coordinate). 613 | 614 | >>> frustum = numpy.random.rand(6) 615 | >>> frustum[1] += frustum[0] 616 | >>> frustum[3] += frustum[2] 617 | >>> frustum[5] += frustum[4] 618 | >>> M = clip_matrix(perspective=False, *frustum) 619 | >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 620 | array([-1., -1., -1., 1.]) 621 | >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) 622 | array([ 1., 1., 1., 1.]) 623 | >>> M = clip_matrix(perspective=True, *frustum) 624 | >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 625 | >>> v / v[3] 626 | array([-1., -1., -1., 1.]) 627 | >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) 628 | >>> v / v[3] 629 | array([ 1., 1., -1., 1.]) 630 | 631 | """ 632 | if left >= right or bottom >= top or near >= far: 633 | raise ValueError('invalid frustum') 634 | if perspective: 635 | if near <= _EPS: 636 | raise ValueError('invalid frustum: near <= 0') 637 | t = 2.0 * near 638 | M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], 639 | [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], 640 | [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], 641 | [0.0, 0.0, -1.0, 0.0]] 642 | else: 643 | M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], 644 | [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], 645 | [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], 646 | [0.0, 0.0, 0.0, 1.0]] 647 | return numpy.array(M) 648 | 649 | 650 | def shear_matrix(angle, direction, point, normal): 651 | """Return matrix to shear by angle along direction vector on shear plane. 652 | 653 | The shear plane is defined by a point and normal vector. The direction 654 | vector must be orthogonal to the plane's normal vector. 655 | 656 | A point P is transformed by the shear matrix into P" such that 657 | the vector P-P" is parallel to the direction vector and its extent is 658 | given by the angle of P-P'-P", where P' is the orthogonal projection 659 | of P onto the shear plane. 660 | 661 | >>> angle = (random.random() - 0.5) * 4*math.pi 662 | >>> direct = numpy.random.random(3) - 0.5 663 | >>> point = numpy.random.random(3) - 0.5 664 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 665 | >>> S = shear_matrix(angle, direct, point, normal) 666 | >>> numpy.allclose(1, numpy.linalg.det(S)) 667 | True 668 | 669 | """ 670 | normal = unit_vector(normal[:3]) 671 | direction = unit_vector(direction[:3]) 672 | if abs(numpy.dot(normal, direction)) > 1e-6: 673 | raise ValueError('direction and normal vectors are not orthogonal') 674 | angle = math.tan(angle) 675 | M = numpy.identity(4) 676 | M[:3, :3] += angle * numpy.outer(direction, normal) 677 | M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction 678 | return M 679 | 680 | 681 | def shear_from_matrix(matrix): 682 | """Return shear angle, direction and plane from shear matrix. 683 | 684 | >>> angle = (random.random() - 0.5) * 4*math.pi 685 | >>> direct = numpy.random.random(3) - 0.5 686 | >>> point = numpy.random.random(3) - 0.5 687 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 688 | >>> S0 = shear_matrix(angle, direct, point, normal) 689 | >>> angle, direct, point, normal = shear_from_matrix(S0) 690 | >>> S1 = shear_matrix(angle, direct, point, normal) 691 | >>> is_same_transform(S0, S1) 692 | True 693 | 694 | """ 695 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 696 | M33 = M[:3, :3] 697 | # normal: cross independent eigenvectors corresponding to the eigenvalue 1 698 | w, V = numpy.linalg.eig(M33) 699 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] 700 | if len(i) < 2: 701 | raise ValueError('no two linear independent eigenvectors found %s' % w) 702 | V = numpy.real(V[:, i]).squeeze().T 703 | lenorm = -1.0 704 | for i0, i1 in ((0, 1), (0, 2), (1, 2)): 705 | n = numpy.cross(V[i0], V[i1]) 706 | w = vector_norm(n) 707 | if w > lenorm: 708 | lenorm = w 709 | normal = n 710 | normal /= lenorm 711 | # direction and angle 712 | direction = numpy.dot(M33 - numpy.identity(3), normal) 713 | angle = vector_norm(direction) 714 | direction /= angle 715 | angle = math.atan(angle) 716 | # point: eigenvector corresponding to eigenvalue 1 717 | w, V = numpy.linalg.eig(M) 718 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 719 | if not len(i): 720 | raise ValueError('no eigenvector corresponding to eigenvalue 1') 721 | point = numpy.real(V[:, i[-1]]).squeeze() 722 | point /= point[3] 723 | return angle, direction, point, normal 724 | 725 | 726 | def decompose_matrix(matrix): 727 | """Return sequence of transformations from transformation matrix. 728 | 729 | matrix : array_like 730 | Non-degenerative homogeneous transformation matrix 731 | 732 | Return tuple of: 733 | scale : vector of 3 scaling factors 734 | shear : list of shear factors for x-y, x-z, y-z axes 735 | angles : list of Euler angles about static x, y, z axes 736 | translate : translation vector along x, y, z axes 737 | perspective : perspective partition of matrix 738 | 739 | Raise ValueError if matrix is of wrong type or degenerative. 740 | 741 | >>> T0 = translation_matrix([1, 2, 3]) 742 | >>> scale, shear, angles, trans, persp = decompose_matrix(T0) 743 | >>> T1 = translation_matrix(trans) 744 | >>> numpy.allclose(T0, T1) 745 | True 746 | >>> S = scale_matrix(0.123) 747 | >>> scale, shear, angles, trans, persp = decompose_matrix(S) 748 | >>> scale[0] 749 | 0.123 750 | >>> R0 = euler_matrix(1, 2, 3) 751 | >>> scale, shear, angles, trans, persp = decompose_matrix(R0) 752 | >>> R1 = euler_matrix(*angles) 753 | >>> numpy.allclose(R0, R1) 754 | True 755 | 756 | """ 757 | M = numpy.array(matrix, dtype=numpy.float64, copy=True).T 758 | if abs(M[3, 3]) < _EPS: 759 | raise ValueError('M[3, 3] is zero') 760 | M /= M[3, 3] 761 | P = M.copy() 762 | P[:, 3] = 0.0, 0.0, 0.0, 1.0 763 | if not numpy.linalg.det(P): 764 | raise ValueError('matrix is singular') 765 | 766 | scale = numpy.zeros((3, )) 767 | shear = [0.0, 0.0, 0.0] 768 | angles = [0.0, 0.0, 0.0] 769 | 770 | if any(abs(M[:3, 3]) > _EPS): 771 | perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) 772 | M[:, 3] = 0.0, 0.0, 0.0, 1.0 773 | else: 774 | perspective = numpy.array([0.0, 0.0, 0.0, 1.0]) 775 | 776 | translate = M[3, :3].copy() 777 | M[3, :3] = 0.0 778 | 779 | row = M[:3, :3].copy() 780 | scale[0] = vector_norm(row[0]) 781 | row[0] /= scale[0] 782 | shear[0] = numpy.dot(row[0], row[1]) 783 | row[1] -= row[0] * shear[0] 784 | scale[1] = vector_norm(row[1]) 785 | row[1] /= scale[1] 786 | shear[0] /= scale[1] 787 | shear[1] = numpy.dot(row[0], row[2]) 788 | row[2] -= row[0] * shear[1] 789 | shear[2] = numpy.dot(row[1], row[2]) 790 | row[2] -= row[1] * shear[2] 791 | scale[2] = vector_norm(row[2]) 792 | row[2] /= scale[2] 793 | shear[1:] /= scale[2] 794 | 795 | if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: 796 | numpy.negative(scale, scale) 797 | numpy.negative(row, row) 798 | 799 | angles[1] = math.asin(-row[0, 2]) 800 | if math.cos(angles[1]): 801 | angles[0] = math.atan2(row[1, 2], row[2, 2]) 802 | angles[2] = math.atan2(row[0, 1], row[0, 0]) 803 | else: 804 | # angles[0] = math.atan2(row[1, 0], row[1, 1]) 805 | angles[0] = math.atan2(-row[2, 1], row[1, 1]) 806 | angles[2] = 0.0 807 | 808 | return scale, shear, angles, translate, perspective 809 | 810 | 811 | def compose_matrix(scale=None, shear=None, angles=None, translate=None, 812 | perspective=None): 813 | """Return transformation matrix from sequence of transformations. 814 | 815 | This is the inverse of the decompose_matrix function. 816 | 817 | Sequence of transformations: 818 | scale : vector of 3 scaling factors 819 | shear : list of shear factors for x-y, x-z, y-z axes 820 | angles : list of Euler angles about static x, y, z axes 821 | translate : translation vector along x, y, z axes 822 | perspective : perspective partition of matrix 823 | 824 | >>> scale = numpy.random.random(3) - 0.5 825 | >>> shear = numpy.random.random(3) - 0.5 826 | >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) 827 | >>> trans = numpy.random.random(3) - 0.5 828 | >>> persp = numpy.random.random(4) - 0.5 829 | >>> M0 = compose_matrix(scale, shear, angles, trans, persp) 830 | >>> result = decompose_matrix(M0) 831 | >>> M1 = compose_matrix(*result) 832 | >>> is_same_transform(M0, M1) 833 | True 834 | 835 | """ 836 | M = numpy.identity(4) 837 | if perspective is not None: 838 | P = numpy.identity(4) 839 | P[3, :] = perspective[:4] 840 | M = numpy.dot(M, P) 841 | if translate is not None: 842 | T = numpy.identity(4) 843 | T[:3, 3] = translate[:3] 844 | M = numpy.dot(M, T) 845 | if angles is not None: 846 | R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') 847 | M = numpy.dot(M, R) 848 | if shear is not None: 849 | Z = numpy.identity(4) 850 | Z[1, 2] = shear[2] 851 | Z[0, 2] = shear[1] 852 | Z[0, 1] = shear[0] 853 | M = numpy.dot(M, Z) 854 | if scale is not None: 855 | S = numpy.identity(4) 856 | S[0, 0] = scale[0] 857 | S[1, 1] = scale[1] 858 | S[2, 2] = scale[2] 859 | M = numpy.dot(M, S) 860 | M /= M[3, 3] 861 | return M 862 | 863 | 864 | def orthogonalization_matrix(lengths, angles): 865 | """Return orthogonalization matrix for crystallographic cell coordinates. 866 | 867 | Angles are expected in degrees. 868 | 869 | The de-orthogonalization matrix is the inverse. 870 | 871 | >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) 872 | >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) 873 | True 874 | >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 875 | >>> numpy.allclose(numpy.sum(O), 43.063229) 876 | True 877 | 878 | """ 879 | a, b, c = lengths 880 | angles = numpy.radians(angles) 881 | sina, sinb, _ = numpy.sin(angles) 882 | cosa, cosb, cosg = numpy.cos(angles) 883 | co = (cosa * cosb - cosg) / (sina * sinb) 884 | return numpy.array([ 885 | [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], 886 | [-a*sinb*co, b*sina, 0.0, 0.0], 887 | [ a*cosb, b*cosa, c, 0.0], 888 | [ 0.0, 0.0, 0.0, 1.0]]) 889 | 890 | 891 | def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): 892 | """Return affine transform matrix to register two point sets. 893 | 894 | v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous 895 | coordinates, where ndims is the dimensionality of the coordinate space. 896 | 897 | If shear is False, a similarity transformation matrix is returned. 898 | If also scale is False, a rigid/Euclidean transformation matrix 899 | is returned. 900 | 901 | By default the algorithm by Hartley and Zissermann [15] is used. 902 | If usesvd is True, similarity and Euclidean transformation matrices 903 | are calculated by minimizing the weighted sum of squared deviations 904 | (RMSD) according to the algorithm by Kabsch [8]. 905 | Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9] 906 | is used, which is slower when using this Python implementation. 907 | 908 | The returned matrix performs rotation, translation and uniform scaling 909 | (if specified). 910 | 911 | >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]] 912 | >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]] 913 | >>> affine_matrix_from_points(v0, v1) 914 | array([[ 0.14549, 0.00062, 675.50008], 915 | [ 0.00048, 0.14094, 53.24971], 916 | [ 0. , 0. , 1. ]]) 917 | >>> T = translation_matrix(numpy.random.random(3)-0.5) 918 | >>> R = random_rotation_matrix(numpy.random.random(3)) 919 | >>> S = scale_matrix(random.random()) 920 | >>> M = concatenate_matrices(T, R, S) 921 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 922 | >>> v0[3] = 1 923 | >>> v1 = numpy.dot(M, v0) 924 | >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) 925 | >>> M = affine_matrix_from_points(v0[:3], v1[:3]) 926 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 927 | True 928 | 929 | More examples in superimposition_matrix() 930 | 931 | """ 932 | v0 = numpy.array(v0, dtype=numpy.float64, copy=True) 933 | v1 = numpy.array(v1, dtype=numpy.float64, copy=True) 934 | 935 | ndims = v0.shape[0] 936 | if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape: 937 | raise ValueError('input arrays are of wrong shape or type') 938 | 939 | # move centroids to origin 940 | t0 = -numpy.mean(v0, axis=1) 941 | M0 = numpy.identity(ndims+1) 942 | M0[:ndims, ndims] = t0 943 | v0 += t0.reshape(ndims, 1) 944 | t1 = -numpy.mean(v1, axis=1) 945 | M1 = numpy.identity(ndims+1) 946 | M1[:ndims, ndims] = t1 947 | v1 += t1.reshape(ndims, 1) 948 | 949 | if shear: 950 | # Affine transformation 951 | A = numpy.concatenate((v0, v1), axis=0) 952 | u, s, vh = numpy.linalg.svd(A.T) 953 | vh = vh[:ndims].T 954 | B = vh[:ndims] 955 | C = vh[ndims:2*ndims] 956 | t = numpy.dot(C, numpy.linalg.pinv(B)) 957 | t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) 958 | M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) 959 | elif usesvd or ndims != 3: 960 | # Rigid transformation via SVD of covariance matrix 961 | u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) 962 | # rotation matrix from SVD orthonormal bases 963 | R = numpy.dot(u, vh) 964 | if numpy.linalg.det(R) < 0.0: 965 | # R does not constitute right handed system 966 | R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) 967 | s[-1] *= -1.0 968 | # homogeneous transformation matrix 969 | M = numpy.identity(ndims+1) 970 | M[:ndims, :ndims] = R 971 | else: 972 | # Rigid transformation matrix via quaternion 973 | # compute symmetric matrix N 974 | xx, yy, zz = numpy.sum(v0 * v1, axis=1) 975 | xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) 976 | xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) 977 | N = [[xx+yy+zz, 0.0, 0.0, 0.0], 978 | [yz-zy, xx-yy-zz, 0.0, 0.0], 979 | [zx-xz, xy+yx, yy-xx-zz, 0.0], 980 | [xy-yx, zx+xz, yz+zy, zz-xx-yy]] 981 | # quaternion: eigenvector corresponding to most positive eigenvalue 982 | w, V = numpy.linalg.eigh(N) 983 | q = V[:, numpy.argmax(w)] 984 | q /= vector_norm(q) # unit quaternion 985 | # homogeneous transformation matrix 986 | M = quaternion_matrix(q) 987 | 988 | if scale and not shear: 989 | # Affine transformation; scale is ratio of RMS deviations from centroid 990 | v0 *= v0 991 | v1 *= v1 992 | M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) 993 | 994 | # move centroids back 995 | M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) 996 | M /= M[ndims, ndims] 997 | return M 998 | 999 | 1000 | def superimposition_matrix(v0, v1, scale=False, usesvd=True): 1001 | """Return matrix to transform given 3D point set into second point set. 1002 | 1003 | v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points. 1004 | 1005 | The parameters scale and usesvd are explained in the more general 1006 | affine_matrix_from_points function. 1007 | 1008 | The returned matrix is a similarity or Euclidean transformation matrix. 1009 | This function has a fast C implementation in transformations.c. 1010 | 1011 | >>> v0 = numpy.random.rand(3, 10) 1012 | >>> M = superimposition_matrix(v0, v0) 1013 | >>> numpy.allclose(M, numpy.identity(4)) 1014 | True 1015 | >>> R = random_rotation_matrix(numpy.random.random(3)) 1016 | >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]] 1017 | >>> v1 = numpy.dot(R, v0) 1018 | >>> M = superimposition_matrix(v0, v1) 1019 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1020 | True 1021 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 1022 | >>> v0[3] = 1 1023 | >>> v1 = numpy.dot(R, v0) 1024 | >>> M = superimposition_matrix(v0, v1) 1025 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1026 | True 1027 | >>> S = scale_matrix(random.random()) 1028 | >>> T = translation_matrix(numpy.random.random(3)-0.5) 1029 | >>> M = concatenate_matrices(T, R, S) 1030 | >>> v1 = numpy.dot(M, v0) 1031 | >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) 1032 | >>> M = superimposition_matrix(v0, v1, scale=True) 1033 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1034 | True 1035 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 1036 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1037 | True 1038 | >>> v = numpy.empty((4, 100, 3)) 1039 | >>> v[:, :, 0] = v0 1040 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 1041 | >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 1042 | True 1043 | 1044 | """ 1045 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] 1046 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] 1047 | return affine_matrix_from_points(v0, v1, shear=False, 1048 | scale=scale, usesvd=usesvd) 1049 | 1050 | 1051 | def euler_matrix(ai, aj, ak, axes='sxyz'): 1052 | """Return homogeneous rotation matrix from Euler angles and axis sequence. 1053 | 1054 | ai, aj, ak : Euler's roll, pitch and yaw angles 1055 | axes : One of 24 axis sequences as string or encoded tuple 1056 | 1057 | >>> R = euler_matrix(1, 2, 3, 'syxz') 1058 | >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) 1059 | True 1060 | >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 1061 | >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) 1062 | True 1063 | >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5) 1064 | >>> for axes in _AXES2TUPLE.keys(): 1065 | ... R = euler_matrix(ai, aj, ak, axes) 1066 | >>> for axes in _TUPLE2AXES.keys(): 1067 | ... R = euler_matrix(ai, aj, ak, axes) 1068 | 1069 | """ 1070 | try: 1071 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] 1072 | except (AttributeError, KeyError): 1073 | _TUPLE2AXES[axes] # validation 1074 | firstaxis, parity, repetition, frame = axes 1075 | 1076 | i = firstaxis 1077 | j = _NEXT_AXIS[i+parity] 1078 | k = _NEXT_AXIS[i-parity+1] 1079 | 1080 | if frame: 1081 | ai, ak = ak, ai 1082 | if parity: 1083 | ai, aj, ak = -ai, -aj, -ak 1084 | 1085 | si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) 1086 | ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) 1087 | cc, cs = ci*ck, ci*sk 1088 | sc, ss = si*ck, si*sk 1089 | 1090 | M = numpy.identity(4) 1091 | if repetition: 1092 | M[i, i] = cj 1093 | M[i, j] = sj*si 1094 | M[i, k] = sj*ci 1095 | M[j, i] = sj*sk 1096 | M[j, j] = -cj*ss+cc 1097 | M[j, k] = -cj*cs-sc 1098 | M[k, i] = -sj*ck 1099 | M[k, j] = cj*sc+cs 1100 | M[k, k] = cj*cc-ss 1101 | else: 1102 | M[i, i] = cj*ck 1103 | M[i, j] = sj*sc-cs 1104 | M[i, k] = sj*cc+ss 1105 | M[j, i] = cj*sk 1106 | M[j, j] = sj*ss+cc 1107 | M[j, k] = sj*cs-sc 1108 | M[k, i] = -sj 1109 | M[k, j] = cj*si 1110 | M[k, k] = cj*ci 1111 | return M 1112 | 1113 | 1114 | def euler_from_matrix(matrix, axes='sxyz'): 1115 | """Return Euler angles from rotation matrix for specified axis sequence. 1116 | 1117 | axes : One of 24 axis sequences as string or encoded tuple 1118 | 1119 | Note that many Euler angle triplets can describe one matrix. 1120 | 1121 | >>> R0 = euler_matrix(1, 2, 3, 'syxz') 1122 | >>> al, be, ga = euler_from_matrix(R0, 'syxz') 1123 | >>> R1 = euler_matrix(al, be, ga, 'syxz') 1124 | >>> numpy.allclose(R0, R1) 1125 | True 1126 | >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) 1127 | >>> for axes in _AXES2TUPLE.keys(): 1128 | ... R0 = euler_matrix(axes=axes, *angles) 1129 | ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 1130 | ... if not numpy.allclose(R0, R1): print(axes, "failed") 1131 | 1132 | """ 1133 | try: 1134 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1135 | except (AttributeError, KeyError): 1136 | _TUPLE2AXES[axes] # validation 1137 | firstaxis, parity, repetition, frame = axes 1138 | 1139 | i = firstaxis 1140 | j = _NEXT_AXIS[i+parity] 1141 | k = _NEXT_AXIS[i-parity+1] 1142 | 1143 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] 1144 | if repetition: 1145 | sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) 1146 | if sy > _EPS: 1147 | ax = math.atan2( M[i, j], M[i, k]) 1148 | ay = math.atan2( sy, M[i, i]) 1149 | az = math.atan2( M[j, i], -M[k, i]) 1150 | else: 1151 | ax = math.atan2(-M[j, k], M[j, j]) 1152 | ay = math.atan2( sy, M[i, i]) 1153 | az = 0.0 1154 | else: 1155 | cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) 1156 | if cy > _EPS: 1157 | ax = math.atan2( M[k, j], M[k, k]) 1158 | ay = math.atan2(-M[k, i], cy) 1159 | az = math.atan2( M[j, i], M[i, i]) 1160 | else: 1161 | ax = math.atan2(-M[j, k], M[j, j]) 1162 | ay = math.atan2(-M[k, i], cy) 1163 | az = 0.0 1164 | 1165 | if parity: 1166 | ax, ay, az = -ax, -ay, -az 1167 | if frame: 1168 | ax, az = az, ax 1169 | return ax, ay, az 1170 | 1171 | 1172 | def euler_from_quaternion(quaternion, axes='sxyz'): 1173 | """Return Euler angles from quaternion for specified axis sequence. 1174 | 1175 | >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) 1176 | >>> numpy.allclose(angles, [0.123, 0, 0]) 1177 | True 1178 | 1179 | """ 1180 | return euler_from_matrix(quaternion_matrix(quaternion), axes) 1181 | 1182 | 1183 | def quaternion_from_euler(ai, aj, ak, axes='sxyz'): 1184 | """Return quaternion from Euler angles and axis sequence. 1185 | 1186 | ai, aj, ak : Euler's roll, pitch and yaw angles 1187 | axes : One of 24 axis sequences as string or encoded tuple 1188 | 1189 | >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') 1190 | >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) 1191 | True 1192 | 1193 | """ 1194 | try: 1195 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1196 | except (AttributeError, KeyError): 1197 | _TUPLE2AXES[axes] # validation 1198 | firstaxis, parity, repetition, frame = axes 1199 | 1200 | i = firstaxis + 1 1201 | j = _NEXT_AXIS[i+parity-1] + 1 1202 | k = _NEXT_AXIS[i-parity] + 1 1203 | 1204 | if frame: 1205 | ai, ak = ak, ai 1206 | if parity: 1207 | aj = -aj 1208 | 1209 | ai /= 2.0 1210 | aj /= 2.0 1211 | ak /= 2.0 1212 | ci = math.cos(ai) 1213 | si = math.sin(ai) 1214 | cj = math.cos(aj) 1215 | sj = math.sin(aj) 1216 | ck = math.cos(ak) 1217 | sk = math.sin(ak) 1218 | cc = ci*ck 1219 | cs = ci*sk 1220 | sc = si*ck 1221 | ss = si*sk 1222 | 1223 | q = numpy.empty((4, )) 1224 | if repetition: 1225 | q[0] = cj*(cc - ss) 1226 | q[i] = cj*(cs + sc) 1227 | q[j] = sj*(cc + ss) 1228 | q[k] = sj*(cs - sc) 1229 | else: 1230 | q[0] = cj*cc + sj*ss 1231 | q[i] = cj*sc - sj*cs 1232 | q[j] = cj*ss + sj*cc 1233 | q[k] = cj*cs - sj*sc 1234 | if parity: 1235 | q[j] *= -1.0 1236 | 1237 | return q 1238 | 1239 | 1240 | def quaternion_about_axis(angle, axis): 1241 | """Return quaternion for rotation about axis. 1242 | 1243 | >>> q = quaternion_about_axis(0.123, [1, 0, 0]) 1244 | >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) 1245 | True 1246 | 1247 | """ 1248 | q = numpy.array([0.0, axis[0], axis[1], axis[2]]) 1249 | qlen = vector_norm(q) 1250 | if qlen > _EPS: 1251 | q *= math.sin(angle/2.0) / qlen 1252 | q[0] = math.cos(angle/2.0) 1253 | return q 1254 | 1255 | 1256 | def quaternion_matrix(quaternion): 1257 | """Return homogeneous rotation matrix from quaternion. 1258 | 1259 | >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) 1260 | >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) 1261 | True 1262 | >>> M = quaternion_matrix([1, 0, 0, 0]) 1263 | >>> numpy.allclose(M, numpy.identity(4)) 1264 | True 1265 | >>> M = quaternion_matrix([0, 1, 0, 0]) 1266 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 1267 | True 1268 | 1269 | """ 1270 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1271 | n = numpy.dot(q, q) 1272 | if n < _EPS: 1273 | return numpy.identity(4) 1274 | q *= math.sqrt(2.0 / n) 1275 | q = numpy.outer(q, q) 1276 | return numpy.array([ 1277 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 1278 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 1279 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 1280 | [ 0.0, 0.0, 0.0, 1.0]]) 1281 | 1282 | 1283 | def quaternion_from_matrix(matrix, isprecise=False): 1284 | """Return quaternion from rotation matrix. 1285 | 1286 | If isprecise is True, the input matrix is assumed to be a precise rotation 1287 | matrix and a faster algorithm is used. 1288 | 1289 | >>> q = quaternion_from_matrix(numpy.identity(4), True) 1290 | >>> numpy.allclose(q, [1, 0, 0, 0]) 1291 | True 1292 | >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) 1293 | >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) 1294 | True 1295 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 1296 | >>> q = quaternion_from_matrix(R, True) 1297 | >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) 1298 | True 1299 | >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], 1300 | ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] 1301 | >>> q = quaternion_from_matrix(R) 1302 | >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) 1303 | True 1304 | >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], 1305 | ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] 1306 | >>> q = quaternion_from_matrix(R) 1307 | >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) 1308 | True 1309 | >>> R = random_rotation_matrix() 1310 | >>> q = quaternion_from_matrix(R) 1311 | >>> is_same_transform(R, quaternion_matrix(q)) 1312 | True 1313 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False), 1314 | ... quaternion_from_matrix(R, isprecise=True)) 1315 | True 1316 | >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0) 1317 | >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False), 1318 | ... quaternion_from_matrix(R, isprecise=True)) 1319 | True 1320 | 1321 | """ 1322 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] 1323 | if isprecise: 1324 | q = numpy.empty((4, )) 1325 | t = numpy.trace(M) 1326 | if t > M[3, 3]: 1327 | q[0] = t 1328 | q[3] = M[1, 0] - M[0, 1] 1329 | q[2] = M[0, 2] - M[2, 0] 1330 | q[1] = M[2, 1] - M[1, 2] 1331 | else: 1332 | i, j, k = 0, 1, 2 1333 | if M[1, 1] > M[0, 0]: 1334 | i, j, k = 1, 2, 0 1335 | if M[2, 2] > M[i, i]: 1336 | i, j, k = 2, 0, 1 1337 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 1338 | q[i] = t 1339 | q[j] = M[i, j] + M[j, i] 1340 | q[k] = M[k, i] + M[i, k] 1341 | q[3] = M[k, j] - M[j, k] 1342 | q = q[[3, 0, 1, 2]] 1343 | q *= 0.5 / math.sqrt(t * M[3, 3]) 1344 | else: 1345 | m00 = M[0, 0] 1346 | m01 = M[0, 1] 1347 | m02 = M[0, 2] 1348 | m10 = M[1, 0] 1349 | m11 = M[1, 1] 1350 | m12 = M[1, 2] 1351 | m20 = M[2, 0] 1352 | m21 = M[2, 1] 1353 | m22 = M[2, 2] 1354 | # symmetric matrix K 1355 | K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], 1356 | [m01+m10, m11-m00-m22, 0.0, 0.0], 1357 | [m02+m20, m12+m21, m22-m00-m11, 0.0], 1358 | [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) 1359 | K /= 3.0 1360 | # quaternion is eigenvector of K that corresponds to largest eigenvalue 1361 | w, V = numpy.linalg.eigh(K) 1362 | q = V[[3, 0, 1, 2], numpy.argmax(w)] 1363 | if q[0] < 0.0: 1364 | numpy.negative(q, q) 1365 | return q 1366 | 1367 | 1368 | def quaternion_multiply(quaternion1, quaternion0): 1369 | """Return multiplication of two quaternions. 1370 | 1371 | >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) 1372 | >>> numpy.allclose(q, [28, -44, -14, 48]) 1373 | True 1374 | 1375 | """ 1376 | w0, x0, y0, z0 = quaternion0 1377 | w1, x1, y1, z1 = quaternion1 1378 | return numpy.array([ 1379 | -x1*x0 - y1*y0 - z1*z0 + w1*w0, 1380 | x1*w0 + y1*z0 - z1*y0 + w1*x0, 1381 | -x1*z0 + y1*w0 + z1*x0 + w1*y0, 1382 | x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) 1383 | 1384 | 1385 | def quaternion_conjugate(quaternion): 1386 | """Return conjugate of quaternion. 1387 | 1388 | >>> q0 = random_quaternion() 1389 | >>> q1 = quaternion_conjugate(q0) 1390 | >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) 1391 | True 1392 | 1393 | """ 1394 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1395 | numpy.negative(q[1:], q[1:]) 1396 | return q 1397 | 1398 | 1399 | def quaternion_inverse(quaternion): 1400 | """Return inverse of quaternion. 1401 | 1402 | >>> q0 = random_quaternion() 1403 | >>> q1 = quaternion_inverse(q0) 1404 | >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) 1405 | True 1406 | 1407 | """ 1408 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1409 | numpy.negative(q[1:], q[1:]) 1410 | return q / numpy.dot(q, q) 1411 | 1412 | 1413 | def quaternion_real(quaternion): 1414 | """Return real part of quaternion. 1415 | 1416 | >>> quaternion_real([3, 0, 1, 2]) 1417 | 3.0 1418 | 1419 | """ 1420 | return float(quaternion[0]) 1421 | 1422 | 1423 | def quaternion_imag(quaternion): 1424 | """Return imaginary part of quaternion. 1425 | 1426 | >>> quaternion_imag([3, 0, 1, 2]) 1427 | array([ 0., 1., 2.]) 1428 | 1429 | """ 1430 | return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) 1431 | 1432 | 1433 | def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): 1434 | """Return spherical linear interpolation between two quaternions. 1435 | 1436 | >>> q0 = random_quaternion() 1437 | >>> q1 = random_quaternion() 1438 | >>> q = quaternion_slerp(q0, q1, 0) 1439 | >>> numpy.allclose(q, q0) 1440 | True 1441 | >>> q = quaternion_slerp(q0, q1, 1, 1) 1442 | >>> numpy.allclose(q, q1) 1443 | True 1444 | >>> q = quaternion_slerp(q0, q1, 0.5) 1445 | >>> angle = math.acos(numpy.dot(q0, q)) 1446 | >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \ 1447 | numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) 1448 | True 1449 | 1450 | """ 1451 | q0 = unit_vector(quat0[:4]) 1452 | q1 = unit_vector(quat1[:4]) 1453 | if fraction == 0.0: 1454 | return q0 1455 | elif fraction == 1.0: 1456 | return q1 1457 | d = numpy.dot(q0, q1) 1458 | if abs(abs(d) - 1.0) < _EPS: 1459 | return q0 1460 | if shortestpath and d < 0.0: 1461 | # invert rotation 1462 | d = -d 1463 | numpy.negative(q1, q1) 1464 | angle = math.acos(d) + spin * math.pi 1465 | if abs(angle) < _EPS: 1466 | return q0 1467 | isin = 1.0 / math.sin(angle) 1468 | q0 *= math.sin((1.0 - fraction) * angle) * isin 1469 | q1 *= math.sin(fraction * angle) * isin 1470 | q0 += q1 1471 | return q0 1472 | 1473 | 1474 | def random_quaternion(rand=None): 1475 | """Return uniform random unit quaternion. 1476 | 1477 | rand: array like or None 1478 | Three independent random variables that are uniformly distributed 1479 | between 0 and 1. 1480 | 1481 | >>> q = random_quaternion() 1482 | >>> numpy.allclose(1, vector_norm(q)) 1483 | True 1484 | >>> q = random_quaternion(numpy.random.random(3)) 1485 | >>> len(q.shape), q.shape[0]==4 1486 | (1, True) 1487 | 1488 | """ 1489 | if rand is None: 1490 | rand = numpy.random.rand(3) 1491 | else: 1492 | assert len(rand) == 3 1493 | r1 = numpy.sqrt(1.0 - rand[0]) 1494 | r2 = numpy.sqrt(rand[0]) 1495 | pi2 = math.pi * 2.0 1496 | t1 = pi2 * rand[1] 1497 | t2 = pi2 * rand[2] 1498 | return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, 1499 | numpy.cos(t1)*r1, numpy.sin(t2)*r2]) 1500 | 1501 | 1502 | def random_rotation_matrix(rand=None): 1503 | """Return uniform random rotation matrix. 1504 | 1505 | rand: array like 1506 | Three independent random variables that are uniformly distributed 1507 | between 0 and 1 for each returned quaternion. 1508 | 1509 | >>> R = random_rotation_matrix() 1510 | >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 1511 | True 1512 | 1513 | """ 1514 | return quaternion_matrix(random_quaternion(rand)) 1515 | 1516 | 1517 | class Arcball(object): 1518 | """Virtual Trackball Control. 1519 | 1520 | >>> ball = Arcball() 1521 | >>> ball = Arcball(initial=numpy.identity(4)) 1522 | >>> ball.place([320, 320], 320) 1523 | >>> ball.down([500, 250]) 1524 | >>> ball.drag([475, 275]) 1525 | >>> R = ball.matrix() 1526 | >>> numpy.allclose(numpy.sum(R), 3.90583455) 1527 | True 1528 | >>> ball = Arcball(initial=[1, 0, 0, 0]) 1529 | >>> ball.place([320, 320], 320) 1530 | >>> ball.setaxes([1, 1, 0], [-1, 1, 0]) 1531 | >>> ball.constrain = True 1532 | >>> ball.down([400, 200]) 1533 | >>> ball.drag([200, 400]) 1534 | >>> R = ball.matrix() 1535 | >>> numpy.allclose(numpy.sum(R), 0.2055924) 1536 | True 1537 | >>> ball.next() 1538 | 1539 | """ 1540 | def __init__(self, initial=None): 1541 | """Initialize virtual trackball control. 1542 | 1543 | initial : quaternion or rotation matrix 1544 | 1545 | """ 1546 | self._axis = None 1547 | self._axes = None 1548 | self._radius = 1.0 1549 | self._center = [0.0, 0.0] 1550 | self._vdown = numpy.array([0.0, 0.0, 1.0]) 1551 | self._constrain = False 1552 | if initial is None: 1553 | self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) 1554 | else: 1555 | initial = numpy.array(initial, dtype=numpy.float64) 1556 | if initial.shape == (4, 4): 1557 | self._qdown = quaternion_from_matrix(initial) 1558 | elif initial.shape == (4, ): 1559 | initial /= vector_norm(initial) 1560 | self._qdown = initial 1561 | else: 1562 | raise ValueError("initial not a quaternion or matrix") 1563 | self._qnow = self._qpre = self._qdown 1564 | 1565 | def place(self, center, radius): 1566 | """Place Arcball, e.g. when window size changes. 1567 | 1568 | center : sequence[2] 1569 | Window coordinates of trackball center. 1570 | radius : float 1571 | Radius of trackball in window coordinates. 1572 | 1573 | """ 1574 | self._radius = float(radius) 1575 | self._center[0] = center[0] 1576 | self._center[1] = center[1] 1577 | 1578 | def setaxes(self, *axes): 1579 | """Set axes to constrain rotations.""" 1580 | if axes is None: 1581 | self._axes = None 1582 | else: 1583 | self._axes = [unit_vector(axis) for axis in axes] 1584 | 1585 | @property 1586 | def constrain(self): 1587 | """Return state of constrain to axis mode.""" 1588 | return self._constrain 1589 | 1590 | @constrain.setter 1591 | def constrain(self, value): 1592 | """Set state of constrain to axis mode.""" 1593 | self._constrain = bool(value) 1594 | 1595 | def down(self, point): 1596 | """Set initial cursor window coordinates and pick constrain-axis.""" 1597 | self._vdown = arcball_map_to_sphere(point, self._center, self._radius) 1598 | self._qdown = self._qpre = self._qnow 1599 | if self._constrain and self._axes is not None: 1600 | self._axis = arcball_nearest_axis(self._vdown, self._axes) 1601 | self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) 1602 | else: 1603 | self._axis = None 1604 | 1605 | def drag(self, point): 1606 | """Update current cursor window coordinates.""" 1607 | vnow = arcball_map_to_sphere(point, self._center, self._radius) 1608 | if self._axis is not None: 1609 | vnow = arcball_constrain_to_axis(vnow, self._axis) 1610 | self._qpre = self._qnow 1611 | t = numpy.cross(self._vdown, vnow) 1612 | if numpy.dot(t, t) < _EPS: 1613 | self._qnow = self._qdown 1614 | else: 1615 | q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] 1616 | self._qnow = quaternion_multiply(q, self._qdown) 1617 | 1618 | def next(self, acceleration=0.0): 1619 | """Continue rotation in direction of last drag.""" 1620 | q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) 1621 | self._qpre, self._qnow = self._qnow, q 1622 | 1623 | def matrix(self): 1624 | """Return homogeneous rotation matrix.""" 1625 | return quaternion_matrix(self._qnow) 1626 | 1627 | 1628 | def arcball_map_to_sphere(point, center, radius): 1629 | """Return unit sphere coordinates from window coordinates.""" 1630 | v0 = (point[0] - center[0]) / radius 1631 | v1 = (center[1] - point[1]) / radius 1632 | n = v0*v0 + v1*v1 1633 | if n > 1.0: 1634 | # position outside of sphere 1635 | n = math.sqrt(n) 1636 | return numpy.array([v0/n, v1/n, 0.0]) 1637 | else: 1638 | return numpy.array([v0, v1, math.sqrt(1.0 - n)]) 1639 | 1640 | 1641 | def arcball_constrain_to_axis(point, axis): 1642 | """Return sphere point perpendicular to axis.""" 1643 | v = numpy.array(point, dtype=numpy.float64, copy=True) 1644 | a = numpy.array(axis, dtype=numpy.float64, copy=True) 1645 | v -= a * numpy.dot(a, v) # on plane 1646 | n = vector_norm(v) 1647 | if n > _EPS: 1648 | if v[2] < 0.0: 1649 | numpy.negative(v, v) 1650 | v /= n 1651 | return v 1652 | if a[2] == 1.0: 1653 | return numpy.array([1.0, 0.0, 0.0]) 1654 | return unit_vector([-a[1], a[0], 0.0]) 1655 | 1656 | 1657 | def arcball_nearest_axis(point, axes): 1658 | """Return axis, which arc is nearest to point.""" 1659 | point = numpy.array(point, dtype=numpy.float64, copy=False) 1660 | nearest = None 1661 | mx = -1.0 1662 | for axis in axes: 1663 | t = numpy.dot(arcball_constrain_to_axis(point, axis), point) 1664 | if t > mx: 1665 | nearest = axis 1666 | mx = t 1667 | return nearest 1668 | 1669 | 1670 | # epsilon for testing whether a number is close to zero 1671 | _EPS = numpy.finfo(float).eps * 4.0 1672 | 1673 | # axis sequences for Euler angles 1674 | _NEXT_AXIS = [1, 2, 0, 1] 1675 | 1676 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 1677 | _AXES2TUPLE = { 1678 | 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 1679 | 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 1680 | 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 1681 | 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 1682 | 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 1683 | 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 1684 | 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 1685 | 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 1686 | 1687 | _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) 1688 | 1689 | 1690 | def vector_norm(data, axis=None, out=None): 1691 | """Return length, i.e. Euclidean norm, of ndarray along axis. 1692 | 1693 | >>> v = numpy.random.random(3) 1694 | >>> n = vector_norm(v) 1695 | >>> numpy.allclose(n, numpy.linalg.norm(v)) 1696 | True 1697 | >>> v = numpy.random.rand(6, 5, 3) 1698 | >>> n = vector_norm(v, axis=-1) 1699 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) 1700 | True 1701 | >>> n = vector_norm(v, axis=1) 1702 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1703 | True 1704 | >>> v = numpy.random.rand(5, 4, 3) 1705 | >>> n = numpy.empty((5, 3)) 1706 | >>> vector_norm(v, axis=1, out=n) 1707 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1708 | True 1709 | >>> vector_norm([]) 1710 | 0.0 1711 | >>> vector_norm([1]) 1712 | 1.0 1713 | 1714 | """ 1715 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1716 | if out is None: 1717 | if data.ndim == 1: 1718 | return math.sqrt(numpy.dot(data, data)) 1719 | data *= data 1720 | out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 1721 | numpy.sqrt(out, out) 1722 | return out 1723 | else: 1724 | data *= data 1725 | numpy.sum(data, axis=axis, out=out) 1726 | numpy.sqrt(out, out) 1727 | 1728 | 1729 | def unit_vector(data, axis=None, out=None): 1730 | """Return ndarray normalized by length, i.e. Euclidean norm, along axis. 1731 | 1732 | >>> v0 = numpy.random.random(3) 1733 | >>> v1 = unit_vector(v0) 1734 | >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 1735 | True 1736 | >>> v0 = numpy.random.rand(5, 4, 3) 1737 | >>> v1 = unit_vector(v0, axis=-1) 1738 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 1739 | >>> numpy.allclose(v1, v2) 1740 | True 1741 | >>> v1 = unit_vector(v0, axis=1) 1742 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 1743 | >>> numpy.allclose(v1, v2) 1744 | True 1745 | >>> v1 = numpy.empty((5, 4, 3)) 1746 | >>> unit_vector(v0, axis=1, out=v1) 1747 | >>> numpy.allclose(v1, v2) 1748 | True 1749 | >>> list(unit_vector([])) 1750 | [] 1751 | >>> list(unit_vector([1])) 1752 | [1.0] 1753 | 1754 | """ 1755 | if out is None: 1756 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1757 | if data.ndim == 1: 1758 | data /= math.sqrt(numpy.dot(data, data)) 1759 | return data 1760 | else: 1761 | if out is not data: 1762 | out[:] = numpy.array(data, copy=False) 1763 | data = out 1764 | length = numpy.atleast_1d(numpy.sum(data*data, axis)) 1765 | numpy.sqrt(length, length) 1766 | if axis is not None: 1767 | length = numpy.expand_dims(length, axis) 1768 | data /= length 1769 | if out is None: 1770 | return data 1771 | 1772 | 1773 | def random_vector(size): 1774 | """Return array of random doubles in the half-open interval [0.0, 1.0). 1775 | 1776 | >>> v = random_vector(10000) 1777 | >>> numpy.all(v >= 0) and numpy.all(v < 1) 1778 | True 1779 | >>> v0 = random_vector(10) 1780 | >>> v1 = random_vector(10) 1781 | >>> numpy.any(v0 == v1) 1782 | False 1783 | 1784 | """ 1785 | return numpy.random.random(size) 1786 | 1787 | 1788 | def vector_product(v0, v1, axis=0): 1789 | """Return vector perpendicular to vectors. 1790 | 1791 | >>> v = vector_product([2, 0, 0], [0, 3, 0]) 1792 | >>> numpy.allclose(v, [0, 0, 6]) 1793 | True 1794 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 1795 | >>> v1 = [[3], [0], [0]] 1796 | >>> v = vector_product(v0, v1) 1797 | >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) 1798 | True 1799 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 1800 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 1801 | >>> v = vector_product(v0, v1, axis=1) 1802 | >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) 1803 | True 1804 | 1805 | """ 1806 | return numpy.cross(v0, v1, axis=axis) 1807 | 1808 | 1809 | def angle_between_vectors(v0, v1, directed=True, axis=0): 1810 | """Return angle between vectors. 1811 | 1812 | If directed is False, the input vectors are interpreted as undirected axes, 1813 | i.e. the maximum angle is pi/2. 1814 | 1815 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) 1816 | >>> numpy.allclose(a, math.pi) 1817 | True 1818 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) 1819 | >>> numpy.allclose(a, 0) 1820 | True 1821 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 1822 | >>> v1 = [[3], [0], [0]] 1823 | >>> a = angle_between_vectors(v0, v1) 1824 | >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) 1825 | True 1826 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 1827 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 1828 | >>> a = angle_between_vectors(v0, v1, axis=1) 1829 | >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) 1830 | True 1831 | 1832 | """ 1833 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False) 1834 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False) 1835 | dot = numpy.sum(v0 * v1, axis=axis) 1836 | dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) 1837 | dot = numpy.clip(dot, -1.0, 1.0) 1838 | return numpy.arccos(dot if directed else numpy.fabs(dot)) 1839 | 1840 | 1841 | def inverse_matrix(matrix): 1842 | """Return inverse of square transformation matrix. 1843 | 1844 | >>> M0 = random_rotation_matrix() 1845 | >>> M1 = inverse_matrix(M0.T) 1846 | >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 1847 | True 1848 | >>> for size in range(1, 7): 1849 | ... M0 = numpy.random.rand(size, size) 1850 | ... M1 = inverse_matrix(M0) 1851 | ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size) 1852 | 1853 | """ 1854 | return numpy.linalg.inv(matrix) 1855 | 1856 | 1857 | def concatenate_matrices(*matrices): 1858 | """Return concatenation of series of transformation matrices. 1859 | 1860 | >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 1861 | >>> numpy.allclose(M, concatenate_matrices(M)) 1862 | True 1863 | >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 1864 | True 1865 | 1866 | """ 1867 | M = numpy.identity(4) 1868 | for i in matrices: 1869 | M = numpy.dot(M, i) 1870 | return M 1871 | 1872 | 1873 | def is_same_transform(matrix0, matrix1): 1874 | """Return True if two matrices perform same transformation. 1875 | 1876 | >>> is_same_transform(numpy.identity(4), numpy.identity(4)) 1877 | True 1878 | >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) 1879 | False 1880 | 1881 | """ 1882 | matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) 1883 | matrix0 /= matrix0[3, 3] 1884 | matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) 1885 | matrix1 /= matrix1[3, 3] 1886 | return numpy.allclose(matrix0, matrix1) 1887 | 1888 | 1889 | def is_same_quaternion(q0, q1): 1890 | """Return True if two quaternions are equal.""" 1891 | q0 = numpy.array(q0) 1892 | q1 = numpy.array(q1) 1893 | return numpy.allclose(q0, q1) or numpy.allclose(q0, -q1) 1894 | 1895 | 1896 | def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): 1897 | """Try import all public attributes from module into global namespace. 1898 | 1899 | Existing attributes with name clashes are renamed with prefix. 1900 | Attributes starting with underscore are ignored by default. 1901 | 1902 | Return True on successful import. 1903 | 1904 | """ 1905 | import warnings 1906 | from importlib import import_module 1907 | try: 1908 | if not package: 1909 | module = import_module(name) 1910 | else: 1911 | module = import_module('.' + name, package=package) 1912 | except ImportError: 1913 | if warn: 1914 | warnings.warn('failed to import module %s' % name) 1915 | else: 1916 | for attr in dir(module): 1917 | if ignore and attr.startswith(ignore): 1918 | continue 1919 | if prefix: 1920 | if attr in globals(): 1921 | globals()[prefix + attr] = globals()[attr] 1922 | elif warn: 1923 | warnings.warn('no Python implementation of ' + attr) 1924 | globals()[attr] = getattr(module, attr) 1925 | return True 1926 | 1927 | 1928 | #_import_module('_transformations') 1929 | 1930 | 1931 | if __name__ == '__main__': 1932 | import doctest 1933 | import random # noqa: used in doctests 1934 | try: 1935 | numpy.set_printoptions(suppress=True, precision=5, legacy='1.13') 1936 | except TypeError: 1937 | numpy.set_printoptions(suppress=True, precision=5) 1938 | doctest.testmod() 1939 | 1940 | --------------------------------------------------------------------------------