def orient_cxn_and_translate(self, xuse, index=None): oriented = [] if (index == None): for i in range(len(self.cxns)): conn_start_ind = 1 + len( self.rods) + i * 6 + i * len(self.cxns[i]) rot_from_angles = trans.compose_matrix( angles=xuse[conn_start_ind:conn_start_ind + 3])[0:4, 0:4] #print(rot_from_angles) rot_from_angles[0, 3] = xuse[conn_start_ind + 3] rot_from_angles[1, 3] = xuse[conn_start_ind + 4] rot_from_angles[2, 3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.cxns)) else: conn_start_ind = 1 + len(self.rods) + index * 6 rot_from_angles = trans.compose_matrix( angles=xuse[conn_start_ind:conn_start_ind + 3])[0:4, 0:4] #print(rot_from_angles) rot_from_angles[0, 3] = xuse[conn_start_ind + 3] rot_from_angles[1, 3] = xuse[conn_start_ind + 4] rot_from_angles[2, 3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.cxns)) return oriented
def orient_molecule_and_translate(self, xuse, index=None): oriented = [] if (index == None): for i in range(len(self.mol.molecule)): # start index is F, {dC}, euler angles and tx,y,z, then the C image of each cxn pt conn_start_ind = 1 + len( self.rods) + i * 6 + i * len(self.cxns[i]) rot_from_angles = trans.compose_matrix( angles=xuse[conn_start_ind:conn_start_ind + 3])[0:4, 0:4] #print(rot_from_angles) rot_from_angles[0, 3] = xuse[conn_start_ind + 3] rot_from_angles[1, 3] = xuse[conn_start_ind + 4] rot_from_angles[2, 3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.molecule)) else: conn_start_ind = 1 + len(self.rods) + index * 6 rot_from_angles = trans.compose_matrix( angles=xuse[conn_start_ind:conn_start_ind + 3])[0:4, 0:4] #print(rot_from_angles) rot_from_angles[0, 3] = xuse[conn_start_ind + 3] rot_from_angles[1, 3] = xuse[conn_start_ind + 4] rot_from_angles[2, 3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.molecule)) return oriented
def test_transform(self): random_e = np.random.rand(3) * np.pi * 2 random_t = np.random.rand(3) pose = Pose(random_t, random_e) matrix = ttf.compose_matrix(angles=random_e, translate=random_t) random_e = np.random.rand(3) * np.pi * 2 random_t = np.random.rand(3) pose_2 = Pose(random_t, random_e) matrix_2 = ttf.compose_matrix(angles=random_e, translate=random_t) matrix_t = matrix_2.dot(matrix) pose_t = pose_2.transform(pose) self.assert_pose_matrix_eq(pose_t, matrix_t)
def stl_objConversion(): # http://www.lfd.uci.edu/~gohlke/code/transformations.py.html import transformations as t_ inputpath = "/home/misumi/Desktop/PCLDevelopment_ConversionTool/Data/MisumiData/Cantilever/STL" outputpath = "/home/misumi/Desktop/PCLDevelopment_ConversionTool/Data/MisumiData/Cantilever/OBJ" listofinputfiles = getListOfFiles(inputpath) for f in listofinputfiles: # for file in f: if ".STL" in f: _ftemp = f.replace(" ", "") getbasepath = os.path.split(_ftemp) basename = getbasepath[0] foldername = os.path.split(basename) createfolder = os.path.join(outputpath,foldername[1]) outputfilename = os.path.join(createfolder,Path(_ftemp).name.split('.')[0] + ".obj") print(outputfilename) createFolder(createfolder) stl = open_stl(f) with open(outputfilename, 'w') as obj: tra = t_.compose_matrix( scale=[0.5, 0.5, 0.5], translate=[-2, 1, 2], angles=[math.pi/4, 0, math.pi/5]) print(tra) stl.transformation = tra stl.to_obj(obj)
def main(): # http://www.lfd.uci.edu/~gohlke/code/transformations.py.html import transformations as t_ #from sys import argv inputpath = "" #path to STL file outputpath = "" #path to OBJ file listofinputfiles = getListOfFiles(inputpath) for f in listofinputfiles: # for file in f: if ".STL" in f: # getbasepath = os.path.split(f) _ftemp = f.replace(" ", "") getbasepath = os.path.split(_ftemp) basename = getbasepath[0] foldername = os.path.split(basename) createfolder = os.path.join(outputpath, foldername[1]) outputfilename = os.path.join( createfolder, Path(_ftemp).name.split('.')[0] + ".obj") print(outputfilename) createFolder(createfolder) stl = open_stl(f) with open(outputfilename, 'w') as obj: tra = t_.compose_matrix(scale=[0.5, 0.5, 0.5], translate=[-2, 1, 2], angles=[math.pi / 4, 0, math.pi / 5]) print(tra) stl.transformation = tra stl.to_obj(obj)
def test_inverse(self): random_e = np.random.rand(3) * np.pi * 2 random_t = np.random.rand(3) pose = Pose(random_t, random_e).inverse() matrix = np.linalg.inv( ttf.compose_matrix(angles=random_e, translate=random_t)) self.assert_pose_matrix_eq(pose, matrix)
def readMhpFile(self, filepath): log.message("Loading MHP file %s", filepath) amt = self.armature fp = open(filepath, "rU") bname = None mat = np.identity(4, float) for line in fp: words = line.split() if len(words) < 4: continue if words[0] != bname: self.setMatrixPose(bname, mat) bname = words[0] mat = np.identity(4, float) if words[1] in ["quat", "gquat"]: quat = float(words[2]),float(words[3]),float(words[4]),float(words[5]) mat = tm.quaternion_matrix(quat) if words[1] == "gquat": pb = self.posebones[words[0]] mat = np.dot(la.inv(pb.bone.matrixRelative), mat) elif words[1] == "scale": scale = 1+float(words[2]), 1+float(words[3]), 1+float(words[4]) smat = tm.compose_matrix(scale=scale) mat = np.dot(smat, mat) elif words[1] == "matrix": rows = [] n = 2 for i in range(4): rows.append((float(words[n]), float(words[n+1]), float(words[n+2]), float(words[n+3]))) n += 4 mat = np.array(rows) self.setMatrixPose(bname, mat) fp.close() self.update()
def local_transform(self): return np.matmul( self.ttrans, np.matmul( self.ctrans, np.matmul(compose_matrix(angles=self.theta_radians), self.ctrans_inv)))
def vertical_plane_of_markers(id_first, rows, cols, spacing, origin_pose): xforms = [ origin_pose @ xf.compose_matrix(None, None, [0, -math.pi / 2, 0], [0, (c - (cols - 1) / 2) * spacing, r * spacing]) for r in range(rows) for c in range(cols) ] decompositions = [xf.decompose_matrix(xform) for xform in xforms] return [[ id_first + idx, d[3][0], d[3][1], d[3][2], d[2][0], d[2][1], d[2][2] ] for idx, d in enumerate(decompositions)]
def dimensionsOLD(gltf, node, parentMatrix=transformations.scale_matrix(1)): # pprint(node) T = node.translation or [0, 0, 0] R = node.rotation or [0, 0, 0, 1] S = node.scale or [1, 1, 1] # thisNodeMatrix = composeMatrixFromTRS(T,R,S) R = transformations.euler_from_quaternion(R, 'szxy') thisNodeMatrix = node.matrix or transformations.compose_matrix( S, None, R, T) thisNodeMatrix = np.array(thisNodeMatrix) thisNodeMatrix.shape = (4, 4) newMatrix = np.dot(parentMatrix, thisNodeMatrix) # newMatrix = np.dot(thisNodeMatrix,parentMatrix) if node.mesh != None: accessor = gltf.accessors[gltf.meshes[ node.mesh].primitives[0].attributes.POSITION] testMin = accessor.min testMax = accessor.max print(testMin) testMin = np.matmul(newMatrix, accessor.min + [1]) print("After multply:", testMin) print(testMax) testMax = np.matmul(newMatrix, accessor.max + [1]) print("After multply:", testMax) for test in [testMin, testMax]: myMin[0] = test[0] if test[0] < myMin[0] else myMin[0] myMin[1] = test[1] if test[1] < myMin[1] else myMin[1] myMin[2] = test[2] if test[2] < myMin[2] else myMin[2] myMax[0] = test[0] if test[0] > myMax[0] else myMax[0] myMax[1] = test[1] if test[1] > myMax[1] else myMax[1] myMax[2] = test[2] if test[2] > myMax[2] else myMax[2] # myMin[0] = testMin[0] if testMin[0] < myMin[0] else myMin[0] # myMin[1] = testMin[1] if testMin[1] < myMin[1] else myMin[1] # myMin[2] = testMin[2] if testMin[2] < myMin[2] else myMin[2] # myMax[0] = testMax[0] if testMax[0] > myMax[0] else myMax[0] # myMax[1] = testMax[1] if testMax[1] > myMax[1] else myMax[1] # myMax[2] = testMax[2] if testMax[2] > myMax[2] else myMax[2] if node.children == None or len(node.children) == 0: # pprint(thisNodeMatrix) # print(node.mesh) return for child in node.children: dimensionsOLD(gltf, gltf.nodes[child], newMatrix)
def _gen_rigid_transform(self, only_translate = False): sign_angles = np.where(np.random.rand(3) < 0.5, -1, 1) angles = np.random.rand(3) * 10 * np.pi / 180 * sign_angles if only_translate: angles = angles * 0 sign_translate = np.where(np.random.rand(3) < 0.5, -1, 1) translate = np.random.rand(3) * self.max_translate * sign_translate affine_mat = trf.compose_matrix(angles=angles, translate=translate) se_3 = [translate[0]/self.max_translate, translate[1]/self.max_translate, translate[2]/self.max_translate, affine_mat[2,1]-affine_mat[1,2], affine_mat[0,2]-affine_mat[2,0], affine_mat[1,0]-affine_mat[0,1]] return affine_mat, se_3
def circle_of_planes_of_markers(id_first, rows, cols, spacing, radius, n): rotated_planes = [] delta_theta = math.pi * 2. / n for i in range(n): theta = delta_theta * i rotated_planes += vertical_plane_of_markers( i * rows * cols + id_first, rows, cols, spacing, xf.compose_matrix( None, None, [0., 0., theta], [radius * math.cos(theta), radius * math.sin(theta), 0.], None)) return rotated_planes
def eliminateAffineScaling(params): # adopt parameters to image spacing params = n.array(params) oldMatrix = n.diag((1.,1.,1.,1.)) oldMatrix[:3,:3] = params[:9].reshape((3,3)) #print oldMatrix decomp = transformations.decompose_matrix(oldMatrix) #print decomp[0] newMatrix = transformations.compose_matrix(scale=n.ones(3),shear=decomp[1],angles=decomp[2]) newParams = newMatrix[:3,:3].flatten() newTrans = params[9:]*n.array([decomp[0][0],1.,1.]) newParams = n.concatenate([newParams,newTrans],0) return newParams
def render(self, run_time, frame_time): if self.first_pass: self.cell_framebuffer.use() #print(self.ctx.fbo) self.ctx.clear(0., 0., 0., 0.) self.ctx.enable(moderngl.BLEND) self.ctx.blend_func = self.ctx.ADDITIVE_BLENDING # Required to add quadrics together self.fp_vao.render(mode=moderngl.POINTS) if not self.first_pass_output: self.first_pass_output = True num_components = 4 ''' print("Screen size:",self.ctx.screen.size, len(self.ctx.screen.read(components=num_components, dtype="f4"))) #print(self.ctx.screen.read(components=num_components, dtype="f4")) raw_data = self.ctx.screen.read(components=num_components, dtype="f4") first_pass_data = np.frombuffer(raw_data, dtype="f4"), print("FP Data Shape:", first_pass_data[0].shape) new_shape = list(self.wnd.size) + [num_components] print("New shape:",new_shape ) first_pass_data = np.reshape(first_pass_data, newshape=new_shape ) print("FP Data Shape:", first_pass_data.shape) ''' #exit() print("Framebuffer size:",len(self.cell_framebuffer.read(components=4, dtype="f4"))) print(self.cell_framebuffer.read(components=4, dtype="f4")[120:140]) first_pass_data = np.reshape( np.frombuffer(self.cell_framebuffer.read(components=4, dtype="f4"), dtype=np.float32), newshape=(self.resolution**2, self.resolution * 4, 4) ) print(first_pass_data.shape) #exit() np.save("first_pass_output_single_point", first_pass_data) #exit() print(os.path.getsize("./first_pass_output_single_point.npy")) self.first_pass_output = True self.first_pass = False #self.cell_framebuffer.release() elif not self.first_pass and self.mini_tris: #self.ctx.clear(self.back_color) self.ctx.clear(0.0, 0.0, 0.0) #self.vao.render(mode=moderngl.POINTS, vertices=100, instances=2) self.vao.render(mode=moderngl.POINTS) self.miniTrisProg['model'].value = tuple(transf.compose_matrix(angles=(0, np.pi/2 * run_time/8, 0)).ravel()) else: self.close() exit()
def orient_cxn_and_translate(self, xuse, index = None): oriented = [] if(index == None): for i in range(len(self.cxns)): conn_start_ind = 1 + len(self.rods) + i*6 + i*len(self.cxns[i]) rot_from_angles = trans.compose_matrix(angles = xuse[conn_start_ind:conn_start_ind+3])[0:4,0:4] #print(rot_from_angles) rot_from_angles[0,3] = xuse[conn_start_ind + 3] rot_from_angles[1,3] = xuse[conn_start_ind + 4] rot_from_angles[2,3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.cxns)) else: conn_start_ind = 1 + len(self.rods) + index*6 rot_from_angles = trans.compose_matrix(angles = xuse[conn_start_ind:conn_start_ind+3])[0:4,0:4] #print(rot_from_angles) rot_from_angles[0,3] = xuse[conn_start_ind + 3] rot_from_angles[1,3] = xuse[conn_start_ind + 4] rot_from_angles[2,3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.cxns)) return oriented
def eliminateAffineScaling(params): # adopt parameters to image spacing params = n.array(params) oldMatrix = n.diag((1., 1., 1., 1.)) oldMatrix[:3, :3] = params[:9].reshape((3, 3)) #print oldMatrix decomp = transformations.decompose_matrix(oldMatrix) #print decomp[0] newMatrix = transformations.compose_matrix(scale=n.ones(3), shear=decomp[1], angles=decomp[2]) newParams = newMatrix[:3, :3].flatten() newTrans = params[9:] * n.array([decomp[0][0], 1., 1.]) newParams = n.concatenate([newParams, newTrans], 0) return newParams
def orient_molecule_and_translate(self, xuse, index = None): oriented = [] if(index == None): for i in range(len(self.mol.molecule)): # start index is F, {dC}, euler angles and tx,y,z, then the C image of each cxn pt conn_start_ind = 1 + len(self.rods) + i*6 + i*len(self.cxns[i]) rot_from_angles = trans.compose_matrix(angles = xuse[conn_start_ind:conn_start_ind+3])[0:4,0:4] #print(rot_from_angles) rot_from_angles[0,3] = xuse[conn_start_ind + 3] rot_from_angles[1,3] = xuse[conn_start_ind + 4] rot_from_angles[2,3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.molecule)) else: conn_start_ind = 1 + len(self.rods) + index*6 rot_from_angles = trans.compose_matrix(angles = xuse[conn_start_ind:conn_start_ind+3])[0:4,0:4] #print(rot_from_angles) rot_from_angles[0,3] = xuse[conn_start_ind + 3] rot_from_angles[1,3] = xuse[conn_start_ind + 4] rot_from_angles[2,3] = xuse[conn_start_ind + 5] oriented.append(np.dot(rot_from_angles, self.mol.molecule)) return oriented
def animate_2dsynced(data, shape_id, figfname): fig, ax = plt.subplots() probe_radius = 0.004745 # probe1: 0.00626/2 probe2: 0.004745 v = int(figfname.split('_')[-4].split('=')[1]) sub = 1 # subsample rate tip_pose = data['tip_poses_2d'] object_pose = data['object_poses_2d'] force = data['force_2d'] patches = [] # add the object as polygon shape_db = ShapeDB() shape_polygon = shape_db.shape_db[shape_id]['shape_poly'] # shape of the objects presented as polygon. shape_polygon_3d = np.hstack((np.array(shape_polygon), np.zeros((len(shape_polygon), 1)), np.ones((len(shape_polygon), 1)))) print 'object_pose', len(object_pose), 'tip_pose', len(tip_pose), 'force', len(force) plt.ion() for i in (range(0, len(tip_pose), sub)): plt.cla() T = tfm.compose_matrix(translate = object_pose[i][0:2] + [0], angles = (0,0,object_pose[i][2]) ) shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T) obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, color='blue', alpha=0.05) ax.add_patch(obj) # add the probes as circle circle = mpatches.Circle(tip_pose[i][0:2], probe_radius, ec="none", color='red', alpha=0.5) ax.add_patch(circle) # add the force ax.arrow(tip_pose[i][0], tip_pose[i][1], force[i][0]/100, force[i][1]/100, head_width=0.005, head_length=0.01, fc='k', ec='k') #arrow = mpatches.Arrow(tip_pose[i][0], tip_pose[i][1], force[i][0], # force[i][1], head_width=0.05, head_length=0.1, fc='k', ec='k') #ax.add_patch(arrow) # render it plt.axis([-0.1, 0.1, -0.1, 0.1]) #plt.axis('equal') plt.draw() #time.sleep(0.1) plt.show()
def update_joint_positions(self): self.root.sum_transform = compose_matrix( angles=self.root.theta_radians, translate=self.root.direction) self.root.base_pos = self.root.offset self.root.end_pos = self.root.offset self.root.sum_ctrans = self.root.ctrans for joint in self.joints: joint.sum_transform = np.matmul(joint.parent.sum_transform, joint.local_transform) joint.base_pos = np.matmul(joint.sum_transform, np.array([0, 0, 0, 1]))[:-1] joint.end_pos = np.matmul(joint.sum_transform, np.append(joint.offset, 1))[:-1]
def composeMatrixFromMatrixAndTRS(matrix, T=[0, 0, 0], R=[0, 0, 0, 1], S=[1, 1, 1]): print('matrix', matrix) print('T', T) print('R', R) print('S', S) R = transformations.euler_from_quaternion(R, 'szxy') trs = transformations.compose_matrix(S, None, R, T) matrix = np.array(matrix) matrix.shape = (4, 4) newMatrix = np.dot(trs, matrix) return newMatrix
def render(self, run_time, frame_time): bc = self.back_color self.ctx.enable(moderngl.CULL_FACE) self.ctx.enable(moderngl.DEPTH_TEST) self.ctx.front_face = 'cw' # I do not currently know why everything is clockwise, but ¯\_(ツ)_/¯ self.ctx.clear( bc[0], bc[1], bc[2], bc[3], ) self.current_tri_vao.render(mode=moderngl.TRIANGLES) if self.show_lines: self.current_line_vao.render(mode=moderngl.LINE_LOOP) if self.is_animated: self.model_matrix = tuple( transf.compose_matrix(scale=(1., 1., 1.), angles=(self.x_angle, run_time * np.pi / 4, 0)).ravel()) self.tri_prog['model'].value = self.model_matrix self.line_prog['model'].value = self.model_matrix
def main(): SHOW_AXES = True SHOW_SCENE_AXES = True SHOW_COIL_AXES = True SHOW_SKIN = True SHOW_BRAIN = True SHOW_COIL = True SHOW_MARKERS = True TRANSF_COIL = True SHOW_PLANE = False SELECT_LANDMARKS = 'scalp' # 'all', 'mri' 'scalp' SAVE_ID = True AFFINE_IMG = True NO_SCALE = True SCREENSHOT = False SHOW_OTHER = False reorder = [0, 2, 1] flipx = [True, False, False] # reorder = [0, 1, 2] # flipx = [False, False, False] # default folder and subject # for Bert image use the translation in the base_affine (fall-back) subj_list = ['VictorSouza', 'JaakkoNieminen', 'AinoTervo', 'JuusoKorhonen', 'BaranAydogan', 'AR', 'Bert'] subj = 0 data_dir = os.environ.get('OneDrive') + r'\vh\eventos\sf 2019\mri_science_factory\{}'.format(subj_list[subj]) # filenames img_file = data_dir + r'\{}.nii'.format(subj_list[subj]) brain_file = data_dir + r'\gm.stl' skin_file = data_dir + r'\gm_sn.stl' if subj == 3: other_file = data_dir + r'\gm.ply' elif subj == 4: other_file = data_dir + r'\tracks.vtp' elif subj == 6: other_file = data_dir + r'\gm.ply' else: other_file = data_dir + r'\gm.stl' # coords = lc.load_nexstim(coord_file) # red, green, blue, maroon (dark red), # olive (shitty green), teal (petrol blue), yellow, orange col = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.], [1., .0, 1.], [.5, .5, 0.], [0., .5, .5], [1., 1., 0.], [1., .4, .0]] # extract image header shape and affine transformation from original nifti file imagedata = nb.squeeze_image(nb.load(img_file)) imagedata = nb.as_closest_canonical(imagedata) imagedata.update_header() pix_dim = imagedata.header.get_zooms() img_shape = imagedata.header.get_data_shape() print("Pixel size: \n") print(pix_dim) print("\nImage shape: \n") print(img_shape) print("\nSform: \n") print(imagedata.get_qform(coded=True)) print("\nQform: \n") print(imagedata.get_sform(coded=True)) print("\nFall-back: \n") print(imagedata.header.get_base_affine()) scale_back, shear_back, angs_back, trans_back, persp_back = tf.decompose_matrix(imagedata.header.get_base_affine()) if AFFINE_IMG: affine = imagedata.affine # affine = imagedata.header.get_base_affine() if NO_SCALE: scale, shear, angs, trans, persp = tf.decompose_matrix(affine) affine = tf.compose_matrix(scale=None, shear=shear, angles=angs, translate=trans, perspective=persp) else: affine = np.identity(4) # affine_I = np.identity(4) # create a camera, render window and renderer camera = vtk.vtkCamera() camera.SetPosition(0, 1000, 0) camera.SetFocalPoint(0, 0, 0) camera.SetViewUp(0, 0, 1) camera.ComputeViewPlaneNormal() camera.Azimuth(90.0) camera.Elevation(10.0) ren = vtk.vtkRenderer() ren.SetActiveCamera(camera) ren.ResetCamera() ren.SetUseDepthPeeling(1) ren.SetOcclusionRatio(0.1) ren.SetMaximumNumberOfPeels(100) camera.Dolly(1.5) ren_win = vtk.vtkRenderWindow() ren_win.AddRenderer(ren) ren_win.SetSize(800, 800) ren_win.SetMultiSamples(0) ren_win.SetAlphaBitPlanes(1) # create a renderwindowinteractor iren = vtk.vtkRenderWindowInteractor() iren.SetRenderWindow(ren_win) # if SELECT_LANDMARKS == 'mri': # # MRI landmarks # coord_mri = [['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Coil Loc'], ['EF max']] # pts_ref = [1, 2, 3, 7, 10] # elif SELECT_LANDMARKS == 'all': # # all coords # coord_mri = [['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Nose/Nasion'], ['Left ear'], ['Right ear'], # ['Coil Loc'], ['EF max']] # pts_ref = [1, 2, 3, 5, 4, 6, 7, 10] # elif SELECT_LANDMARKS == 'scalp': # # scalp landmarks # coord_mri = [['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Coil Loc'], ['EF max']] # hdr_mri = ['Nose/Nasion', 'Left ear', 'Right ear', 'Coil Loc', 'EF max'] # pts_ref = [5, 4, 6, 7, 10] # # coords_np = np.zeros([len(pts_ref), 3]) # for n, pts_id in enumerate(pts_ref): # # to keep in the MRI space use the identity as the affine # # coord_aux = n2m.coord_change(coords[pts_id][1:], img_shape, affine_I, flipx, reorder) # # affine_trans = affine_I.copy() # # affine_trans = affine.copy() # # affine_trans[:3, -1] = affine[:3, -1] # coord_aux = n2m.coord_change(coords[pts_id][1:], img_shape, affine, flipx, reorder) # coords_np[n, :] = coord_aux # [coord_mri[n].append(s) for s in coord_aux] # if SHOW_MARKERS: # marker_actor = add_marker(coord_aux, ren, col[n]) # # print('\nOriginal coordinates from Nexstim: \n') # [print(s) for s in coords] # print('\nTransformed coordinates to MRI space: \n') # [print(s) for s in coord_mri] # # # coil location, normal vector and direction vector # coil_loc = coord_mri[-2][1:] # coil_norm = coords[8][1:] # coil_dir = coords[9][1:] # # # creating the coil coordinate system by adding a point in the direction of each given coil vector # # the additional vector is just the cross product from coil direction and coil normal vectors # # origin of the coordinate system is the coil location given by Nexstim # # the vec_length is to allow line creation with visible length in VTK scene # vec_length = 75 # p1 = coords[7][1:] # p2 = [x + vec_length * y for x, y in zip(p1, coil_norm)] # p2_norm = n2m.coord_change(p2, img_shape, affine, flipx, reorder) # # p2 = [x + vec_length * y for x, y in zip(p1, coil_dir)] # p2_dir = n2m.coord_change(p2, img_shape, affine, flipx, reorder) # # coil_face = np.cross(coil_norm, coil_dir) # p2 = [x - vec_length * y for x, y in zip(p1, coil_face.tolist())] # p2_face = n2m.coord_change(p2, img_shape, affine, flipx, reorder) # Coil face unit vector (X) # u1 = np.asarray(p2_face) - np.asarray(coil_loc) # u1_n = u1 / np.linalg.norm(u1) # # Coil direction unit vector (Y) # u2 = np.asarray(p2_dir) - np.asarray(coil_loc) # u2_n = u2 / np.linalg.norm(u2) # # Coil normal unit vector (Z) # u3 = np.asarray(p2_norm) - np.asarray(coil_loc) # u3_n = u3 / np.linalg.norm(u3) # # transf_matrix = np.identity(4) # if TRANSF_COIL: # transf_matrix[:3, 0] = u1_n # transf_matrix[:3, 1] = u2_n # transf_matrix[:3, 2] = u3_n # transf_matrix[:3, 3] = coil_loc[:] # the absolute value of the determinant indicates the scaling factor # the sign of the determinant indicates how it affects the orientation: if positive maintain the # original orientation and if negative inverts all the orientations (flip the object inside-out)' # the negative determinant is what makes objects in VTK scene to become black # print('Transformation matrix: \n', transf_matrix, '\n') # print('Determinant: ', np.linalg.det(transf_matrix)) # if SAVE_ID: # coord_dict = {'m_affine': transf_matrix, 'coords_labels': hdr_mri, 'coords': coords_np} # io.savemat(output_file + '.mat', coord_dict) # hdr_names = ';'.join(['m' + str(i) + str(j) for i in range(1, 5) for j in range(1, 5)]) # np.savetxt(output_file + '.txt', transf_matrix.reshape([1, 16]), delimiter=';', header=hdr_names) if SHOW_BRAIN: # brain_actor = load_stl(brain_file, ren, colour=[0., 1., 1.], opacity=0.7, user_matrix=np.linalg.inv(affine)) affine_orig = np.identity(4) # affine_orig = affine.copy() # affine_orig[0, 3] = affine_orig[0, 3] + pix_dim[0]*img_shape[0] # affine_orig[1, 3] = affine_orig[1, 3] + pix_dim[1]*img_shape[1] # affine_orig[0, 3] = affine_orig[0, 3] + pix_dim[0]*img_shape[0] # affine_orig[0, 3] = affine_orig[0, 3] - 5 # this partially works for DTI Baran # modified close to correct [-75.99139404 123.88291931 - 148.19839478] # fall-back [87.50042766 - 127.5 - 127.5] # affine_orig[0, 3] = -trans_back[0] # affine_orig[1, 3] = -trans_back[1] # this works for the bert image # affine_orig[0, 3] = -127 # affine_orig[1, 3] = 127 # affine_orig[2, 3] = -127 # affine_orig[:3, :3] = affine[:3, :3] # affine_orig[1, 3] = -affine_orig[1, 3]+27.5 # victorsouza # affine_orig[1, 3] = -affine_orig[1, 3]+97.5 # affine_orig[1, 3] = -affine_orig[1, 3] print('Affine original: \n', affine) scale, shear, angs, trans, persp = tf.decompose_matrix(affine) print('Angles: \n', np.rad2deg(angs)) print('Translation: \n', trans) print('Affine modified: \n', affine_orig) scale, shear, angs, trans, persp = tf.decompose_matrix(affine_orig) print('Angles: \n', np.rad2deg(angs)) print('Translation: \n', trans) # colour=[0., 1., 1.], brain_actor, brain_mesh = load_stl(brain_file, ren, replace=True, colour=[1., 0., 0.], opacity=.3, user_matrix=affine_orig) # print('Actor origin: \n', brain_actor.GetPosition()) if SHOW_SKIN: # skin_actor = load_stl(skin_file, ren, opacity=0.5, user_matrix=np.linalg.inv(affine)) # affine[0, 3] = affine[0, 3] + pix_dim[0] * img_shape[0] # this is working # affine[0, 3] = affine[0, 3] + 8. affine[1, 3] = affine[1, 3] + pix_dim[1] * img_shape[1] # affine[2, 3] = affine[2, 3] + pix_dim[2] * img_shape[2] affine_inv = np.linalg.inv(affine) # affine_inv[:3, 3] = -affine[:3, 3] # affine_inv[2, 3] = -affine_inv[2, 3] skin_actor, skin_mesh = load_stl(skin_file, ren, colour="SkinColor", opacity=1., user_matrix=affine_inv) # skin_actor, skin_mesh = load_stl(skin_file, ren, colour="SkinColor", opacity=1.) skino_actor, skino_mesh = load_stl(skin_file, ren, colour=[1., 0., 0.], opacity=1.) if SHOW_OTHER: # skin_actor = load_stl(skin_file, ren, opacity=0.5, user_matrix=np.linalg.inv(affine)) affine[1, 3] = affine[1, 3] + pix_dim[1] * img_shape[1] affine_inv = np.linalg.inv(affine) # affine_inv[:3, 3] = -affine[:3, 3] affine_inv[1, 3] = affine_inv[1, 3] # other_actor, other_mesh = load_stl(other_file, ren, opacity=1., user_matrix=affine_inv) # other_actor, other_mesh = load_stl(other_file, ren, opacity=1.) # if SHOW_COIL: # # reposition STL object prior to transformation matrix # # [translation_x, translation_y, translation_z, rotation_x, rotation_y, rotation_z] # # old translation when using Y as normal vector # # repos = [0., -6., 0., 0., -90., 90.] # # Translate coil loc coordinate to coil bottom # # repos = [0., 0., 5.5, 0., 0., 180.] # repos = [0., 0., 0., 0., 0., 180.] # act_coil = load_stl(coil_file, ren, replace=repos, user_matrix=transf_matrix, opacity=.3) # # if SHOW_PLANE: # act_plane = add_plane(ren, user_matrix=transf_matrix) # Add axes to scene origin if SHOW_AXES: add_line(ren, [0, 0, 0], [150, 0, 0], color=[1.0, 0.0, 0.0]) add_line(ren, [0, 0, 0], [0, 150, 0], color=[0.0, 1.0, 0.0]) add_line(ren, [0, 0, 0], [0, 0, 150], color=[0.0, 0.0, 1.0]) # Add axes to object origin # if SHOW_COIL_AXES: # add_line(ren, coil_loc, p2_norm, color=[.0, .0, 1.0]) # add_line(ren, coil_loc, p2_dir, color=[.0, 1.0, .0]) # add_line(ren, coil_loc, p2_face, color=[1.0, .0, .0]) # Add interactive axes to scene if SHOW_SCENE_AXES: axes = vtk.vtkAxesActor() widget = vtk.vtkOrientationMarkerWidget() widget.SetOutlineColor(0.9300, 0.5700, 0.1300) widget.SetOrientationMarker(axes) widget.SetInteractor(iren) # widget.SetViewport(0.0, 0.0, 0.4, 0.4) widget.SetEnabled(1) widget.InteractiveOn() # # if SCREENSHOT: # # screenshot of VTK scene # w2if = vtk.vtkWindowToImageFilter() # w2if.SetInput(ren_win) # w2if.Update() # # writer = vtk.vtkPNGWriter() # writer.SetFileName("screenshot.png") # writer.SetInput(w2if.GetOutput()) # writer.Write() # Enable user interface interactor # ren_win.Render() ren.ResetCameraClippingRange() iren.Initialize() iren.Start()
def __update_ctrans(self): self.ctrans = compose_matrix(angles=self.axis_radians, translate=[0, 0, 0]) self.ctrans_inv = np.linalg.inv(self.ctrans)
def main(): """ Visualize Freesurfer, SimNIBS headreco, and Nexstim coil locations in the scanner coordinate system. """ SHOW_AXES = True SHOW_SCENE_AXES = True SHOW_COIL_AXES = True SHOW_SKIN = True SHOW_BRAIN = True SHOW_FREESURFER = True SHOW_COIL = True SHOW_MARKERS = True TRANSF_COIL = True SHOW_PLANE = False SELECT_LANDMARKS = 'scalp' # 'all', 'mri' 'scalp' SAVE_ID = False AFFINE_IMG = True NO_SCALE = True SCREENSHOT = False reorder = [0, 2, 1] flipx = [True, False, False] # reorder = [0, 1, 2] # flipx = [False, False, False] # default folder and subject # subj = 's03' subj = 'S5' id_extra = False # 8, 9, 10, 12, False data_dir = os.environ['OneDrive'] + r'\data\nexstim_coord' # data_dir = 'P:\\tms_eeg\\mTMS\\projects\\lateral ppTMS M1\\E-fields\\' # data_subj = data_dir + subj + '\\' simnibs_dir = data_dir + r'\simnibs\m2m_ppM1_{}_nc'.format(subj) fs_dir = data_dir + r'\freesurfer\ppM1_{}'.format(subj) if id_extra: nav_dir = data_dir + r'\nav_coordinates\ppM1_{}_{}'.format( subj, id_extra) else: nav_dir = data_dir + r'\nav_coordinates\ppM1_{}'.format(subj) # filenames # coil_file = data_dir + 'magstim_fig8_coil.stl' coil_file = os.environ[ 'OneDrive'] + r'\data\nexstim_coord\magstim_fig8_coil.stl' if id_extra: coord_file = nav_dir + r'\ppM1_eximia_{}_{}.txt'.format(subj, id_extra) else: coord_file = nav_dir + r'\ppM1_eximia_{}.txt'.format(subj) # img_file = data_subj + subj + '.nii' img_file = data_dir + r'\mri\ppM1_{}\ppM1_{}.nii'.format(subj, subj) brain_file = simnibs_dir + r'\wm.stl' skin_file = simnibs_dir + r'\skin.stl' fs_file = fs_dir + r'\lh.pial.stl' fs_t1 = fs_dir + r'\mri\T1.mgz' if id_extra: output_file = nav_dir + r'\transf_mat_{}_{}'.format(subj, id_extra) else: output_file = nav_dir + r'\transf_mat_{}'.format(subj) coords = lc.load_nexstim(coord_file) # red, green, blue, maroon (dark red), # olive (shitty green), teal (petrol blue), yellow, orange col = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.], [1., .0, 1.], [.5, .5, 0.], [0., .5, .5], [1., 1., 0.], [1., .4, .0]] # extract image header shape and affine transformation from original nifti file imagedata = nb.squeeze_image(nb.load(img_file)) imagedata = nb.as_closest_canonical(imagedata) imagedata.update_header() pix_dim = imagedata.header.get_zooms() img_shape = imagedata.header.get_data_shape() print("Pixel size: \n") print(pix_dim) print("\nImage shape: \n") print(img_shape) if AFFINE_IMG: affine = imagedata.affine if NO_SCALE: scale, shear, angs, trans, persp = tf.decompose_matrix( imagedata.affine) affine = tf.compose_matrix(scale=None, shear=shear, angles=angs, translate=trans, perspective=persp) print("\nAffine: \n") print(affine) else: affine = np.identity(4) # affine_I = np.identity(4) # create a camera, render window and renderer camera = vtk.vtkCamera() camera.SetPosition(0, 1000, 0) camera.SetFocalPoint(0, 0, 0) camera.SetViewUp(0, 0, 1) camera.ComputeViewPlaneNormal() camera.Azimuth(90.0) camera.Elevation(10.0) ren = vtk.vtkRenderer() ren.SetActiveCamera(camera) ren.ResetCamera() camera.Dolly(1.5) ren_win = vtk.vtkRenderWindow() ren_win.AddRenderer(ren) ren_win.SetSize(800, 800) # create a renderwindowinteractor iren = vtk.vtkRenderWindowInteractor() iren.SetRenderWindow(ren_win) if SELECT_LANDMARKS == 'mri': # MRI landmarks coord_mri = [['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Coil Loc'], ['EF max']] pts_ref = [1, 2, 3, 7, 10] elif SELECT_LANDMARKS == 'all': # all coords coord_mri = [['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Coil Loc'], ['EF max']] pts_ref = [1, 2, 3, 5, 4, 6, 7, 10] elif SELECT_LANDMARKS == 'scalp': # scalp landmarks coord_mri = [['Nose/Nasion'], ['Left ear'], ['Right ear'], ['Coil Loc'], ['EF max']] hdr_mri = [ 'Nose/Nasion', 'Left ear', 'Right ear', 'Coil Loc', 'EF max' ] pts_ref = [5, 4, 6, 7, 10] coords_np = np.zeros([len(pts_ref), 3]) for n, pts_id in enumerate(pts_ref): # to keep in the MRI space use the identity as the affine # coord_aux = n2m.coord_change(coords[pts_id][1:], img_shape, affine_I, flipx, reorder) # affine_trans = affine_I.copy() # affine_trans = affine.copy() # affine_trans[:3, -1] = affine[:3, -1] coord_aux = n2m.coord_change(coords[pts_id][1:], img_shape, affine, flipx, reorder) coords_np[n, :] = coord_aux [coord_mri[n].append(s) for s in coord_aux] if SHOW_MARKERS: marker_actor = add_marker(coord_aux, ren, col[n]) print('\nOriginal coordinates from Nexstim: \n') [print(s) for s in coords] print('\nTransformed coordinates to MRI space: \n') [print(s) for s in coord_mri] # coil location, normal vector and direction vector coil_loc = coord_mri[-2][1:] coil_norm = coords[8][1:] coil_dir = coords[9][1:] # creating the coil coordinate system by adding a point in the direction of each given coil vector # the additional vector is just the cross product from coil direction and coil normal vectors # origin of the coordinate system is the coil location given by Nexstim # the vec_length is to allow line creation with visible length in VTK scene vec_length = 75 p1 = coords[7][1:] p2 = [x + vec_length * y for x, y in zip(p1, coil_norm)] p2_norm = n2m.coord_change(p2, img_shape, affine, flipx, reorder) p2 = [x + vec_length * y for x, y in zip(p1, coil_dir)] p2_dir = n2m.coord_change(p2, img_shape, affine, flipx, reorder) coil_face = np.cross(coil_norm, coil_dir) p2 = [x - vec_length * y for x, y in zip(p1, coil_face.tolist())] p2_face = n2m.coord_change(p2, img_shape, affine, flipx, reorder) # Coil face unit vector (X) u1 = np.asarray(p2_face) - np.asarray(coil_loc) u1_n = u1 / np.linalg.norm(u1) # Coil direction unit vector (Y) u2 = np.asarray(p2_dir) - np.asarray(coil_loc) u2_n = u2 / np.linalg.norm(u2) # Coil normal unit vector (Z) u3 = np.asarray(p2_norm) - np.asarray(coil_loc) u3_n = u3 / np.linalg.norm(u3) transf_matrix = np.identity(4) if TRANSF_COIL: transf_matrix[:3, 0] = u1_n transf_matrix[:3, 1] = u2_n transf_matrix[:3, 2] = u3_n transf_matrix[:3, 3] = coil_loc[:] # the absolute value of the determinant indicates the scaling factor # the sign of the determinant indicates how it affects the orientation: if positive maintain the # original orientation and if negative inverts all the orientations (flip the object inside-out)' # the negative determinant is what makes objects in VTK scene to become # black print('Transformation matrix: \n', transf_matrix, '\n') print('Determinant: ', np.linalg.det(transf_matrix)) if SAVE_ID: coord_dict = { 'm_affine': transf_matrix, 'coords_labels': hdr_mri, 'coords': coords_np } io.savemat(output_file + '.mat', coord_dict) hdr_names = ';'.join( ['m' + str(i) + str(j) for i in range(1, 5) for j in range(1, 5)]) np.savetxt(output_file + '.txt', transf_matrix.reshape([1, 16]), delimiter=';', header=hdr_names) if SHOW_BRAIN: # brain_actor = load_stl(brain_file, ren, colour=[0., 1., 1.], opacity=0.7, user_matrix=np.linalg.inv(affine)) brain_actor = load_stl(brain_file, ren, colour=[0., 1., 1.], opacity=1.) if SHOW_SKIN: # skin_actor = load_stl(skin_file, ren, opacity=0.5, user_matrix=np.linalg.inv(affine)) skin_actor = load_stl(skin_file, ren, colour="SkinColor", opacity=.4) if SHOW_FREESURFER: img = fsio.MGHImage.load(fs_t1) #print("MGH Header: ", img) #print("MGH data: ", img.header['Pxyz_c']) # skin_actor = load_stl(skin_file, ren, opacity=0.5, user_matrix=np.linalg.inv(affine)) trans_fs = np.identity(4) trans_fs[:3, -1] = img.header['Pxyz_c'] fs_actor = load_stl(fs_file, ren, colour=[1., 0., 1.], opacity=0.5, user_matrix=trans_fs) if SHOW_COIL: # reposition STL object prior to transformation matrix # [translation_x, translation_y, translation_z, rotation_x, rotation_y, rotation_z] # old translation when using Y as normal vector # repos = [0., -6., 0., 0., -90., 90.] # Translate coil loc coordinate to coil bottom # repos = [0., 0., 5.5, 0., 0., 180.] repos = [0., 0., 0., 0., 0., 180.] act_coil = load_stl(coil_file, ren, replace=repos, user_matrix=transf_matrix, opacity=.3) if SHOW_PLANE: act_plane = add_plane(ren, user_matrix=transf_matrix) # Add axes to scene origin if SHOW_AXES: add_line(ren, [0, 0, 0], [150, 0, 0], color=[1.0, 0.0, 0.0]) add_line(ren, [0, 0, 0], [0, 150, 0], color=[0.0, 1.0, 0.0]) add_line(ren, [0, 0, 0], [0, 0, 150], color=[0.0, 0.0, 1.0]) # Add axes to object origin if SHOW_COIL_AXES: add_line(ren, coil_loc, p2_norm, color=[.0, .0, 1.0]) add_line(ren, coil_loc, p2_dir, color=[.0, 1.0, .0]) add_line(ren, coil_loc, p2_face, color=[1.0, .0, .0]) # Add interactive axes to scene if SHOW_SCENE_AXES: axes = vtk.vtkAxesActor() widget = vtk.vtkOrientationMarkerWidget() widget.SetOutlineColor(0.9300, 0.5700, 0.1300) widget.SetOrientationMarker(axes) widget.SetInteractor(iren) # widget.SetViewport(0.0, 0.0, 0.4, 0.4) widget.SetEnabled(1) widget.InteractiveOn() if SCREENSHOT: # screenshot of VTK scene w2if = vtk.vtkWindowToImageFilter() w2if.SetInput(ren_win) w2if.Update() writer = vtk.vtkPNGWriter() writer.SetFileName("screenshot.png") writer.SetInput(w2if.GetOutput()) writer.Write() # Enable user interface interactor # ren_win.Render() ren.ResetCameraClippingRange() iren.Initialize() iren.Start()
def __update_ttrans(self): self.ttrans = compose_matrix(translate=self.parent.offset) self.ttrans_inv = np.linalg.inv(self.ttrans)
def __init__(self, vertices=None, indices=None, **kwargs ): super().__init__(**kwargs) self.back_color = (1.0, 1.0, 1.0, 1.0) #(1,1,1, 1) if vertices is None: print("Cannot render 0 vertices!") return None if self.mini_tris: self.miniTrisProg['bbox.min'].value = bbox[0] self.miniTrisProg['bbox.max'].value = bbox[1] #print(tuple(transf.compose_matrix(angles=(0, np.pi/2, 0)).ravel())) self.miniTrisProg['model'].value = tuple(transf.compose_matrix(angles=(0, np.pi/2, 0)).ravel()) self.miniTrisProg['view'].value = tuple(transf.identity_matrix().ravel()) self.miniTrisProg['proj'].value = tuple(transf.identity_matrix().ravel()) indices = np.array(self.obj_mesh.mesh_list[0].faces) #print('Vertices 0-9', vertices[:10]) #print('Indices 0-9', indices[:10]) ''' vertices = np.array([ -0.5, -0.5, 0, 0.5, -0.5, 0, -0.5, 0.5, 0, 0.5, 0.5, 0, ], dtype='f4') # https://github.com/moderngl/moderngl/blob/master/examples/raymarching.py idx_data = np.array([ 0, 1, 2, 3 ]) idx_buffer = self.ctx.buffer(idx_data) ''' self.vbo = self.ctx.buffer(vertices) print(self.vbo) self.vao = self.ctx.vertex_array( self.miniTrisProg, [ (self.vbo, '3f', 'inVert'), ] ) elif self.first_pass: self.cluster_quadric_map_generation_prog['bbox.min'].value = bbox[0] self.cluster_quadric_map_generation_prog['bbox.max'].value = bbox[1] #self.cluster_quadric_map_generation_prog['cell_full_scale'].value = self.resolution #self.cluster_quadric_map_generation_prog['resolution'].value = self.resolution self.wnd.size = (self.resolution**2, self.resolution) self.wnd.resize(self.resolution**2, self.resolution) #print(self.wnd.size) #exit() #self.cluster_quadric_map_generation_prog['width'].value = self.wnd.width #self.cluster_quadric_map_generation_prog['height'].value = self.wnd.height index_buffer = self.ctx.buffer(indices) #print('Vertices 0-9', vertices[:10]) #print('Indices 0-9', indices[:10]) # Initialize an empty array texture for octree self.cell_texture = self.ctx.texture( size=(self.resolution**2,self.resolution * 4), components=4, data=np.zeros(self.resolution**3 * 4 * 4,dtype=np.float32, order='C'), alignment=1, dtype="f4") ''' May be used later? self.vertex_texture = self.ctx.texture(size=(len(vertices),1), components=4, data=np.zeros((len(vertices)*4),dtype="f4", order='C'), alignment=1, dtype="f4") ''' print("cell texture size =",self.cell_texture.size,"components=",self.cell_texture.components) #exit() self.cell_framebuffer = self.ctx.framebuffer(color_attachments=self.cell_texture) #print("Framebuffer for cell: Size =",self.cell_framebuffer.size, "viewport =",self.cell_framebuffer.viewport) #self.cell_framebuffer.use() self.vbo = self.ctx.buffer(vertices) #print(self.vbo) self.fp_vao = self.ctx.vertex_array( self.cluster_quadric_map_generation_prog, [ (self.vbo, '3f', 'inVert'), ], )
def main(args): # load object urdf file names, tower poses, and block dimensions from csv csv_data = [] poses_path = os.path.join(args.tower_dir, 'obj_poses.csv') with open(poses_path) as csv_file: csv_reader = csv.reader(csv_file, delimiter=',') for row in csv_reader: pos = [float(v) for v in row[1:4]] orn = [float(v) for v in row[4:8]] dims = [float(d) for d in row[8:11]] row_info = (row[0], pos, orn, dims) csv_data.append(row_info) # copy urdf files to where pb_robot expects them (will remove them later) for (urdf_filename, _, _, _) in csv_data: pb_robot_path = os.path.join(os.getcwd(), 'pb_robot/' + urdf_filename) urdf_path = os.path.join(os.path.join(args.tower_dir, 'urdfs'), urdf_filename) copyfile(urdf_path, pb_robot_path) # start pybullet utils.connect(use_gui=True) utils.disable_real_time() utils.set_default_camera() utils.enable_gravity() # Add floor object plane_id = p.loadURDF("plane_files/plane.urdf", (0., 0., 0.)) # Create robot object and get transform of robot hand in EE frame robot = pb_robot.panda.Panda() hand_id = 10 p_hand_world, q_hand_world = p.getLinkState(robot.id, hand_id)[:2] p_ee_world, q_ee_world = robot.arm.eeFrame.get_link_pose() p_hand_ee = transformation(p_hand_world, p_ee_world, q_ee_world, inverse=True) q_hand_ee = quat_math(q_ee_world, q_hand_world, True, False) # the hand frame is a little inside the hand, want it to be at the point # where the hand and finger meet p_offset = [0., 0., 0.01] trans_hand_ee = compose_matrix(translate=np.add(p_hand_ee, p_offset), angles=euler_from_quaternion(q_hand_ee)) # params num_blocks = len(csv_data) tower_offset = (0.3, 0.0, 0.0) # tower center in x-y plane block_spread_y = 0.6 # width of block initial placement area in y dimension delta_y = block_spread_y / num_blocks # distance between blocks in y dimension min_y = -block_spread_y / 2 # minimum block y dimension xp_com_world_init = 0.6 # initial position of blocks in x dimension for n, row in enumerate(csv_data): # unpack csv data urdf_filename = row[0] p_com_world_goal = row[1] q_com_world_goal = row[2] dims = row[3] # place block in world (on floor at goal orientation) block = pb_robot.body.createBody(urdf_filename) p_com_cog = p.getDynamicsInfo(block.id, -1)[3] up = get_up_axis(q_com_world_goal) zp_com_world_init = dims[up] / 2 + p_com_cog[up] yp_com_world_init = min_y + delta_y * n p_com_world_init = [ xp_com_world_init, yp_com_world_init, zp_com_world_init ] block.set_pose((p_com_world_init, q_com_world_goal)) # transformation from robot EE to object com when grasped (top down grasp) # needed when defining Grab() q_cog_hand = quat_math(q_com_world_goal, q_hand_world, True, False) trans_cog_hand = compose_matrix(translate=[0., 0., dims[up] / 2], angles=[0., np.pi, np.pi]) #angles=q_cog_hand) trans_cog_ee = concatenate_matrices(trans_hand_ee, trans_cog_hand) trans_com_cog = compose_matrix(translate=p_com_cog, angles=[0., 0., 0., 1.]) trans_com_ee = concatenate_matrices(trans_cog_ee, trans_com_cog) # transformation of ee in world frame (at initial block position) trans_com_world_init = compose_matrix(translate=p_com_world_init, angles=q_com_world_goal) trans_ee_world_init = concatenate_matrices(trans_com_world_init, trans_com_ee) # grasp block robot.arm.hand.Open() grasp_q = robot.arm.ComputeIK(trans_ee_world_init) if grasp_q is not None: utils.wait_for_user("Move to desired pose?") robot.arm.SetJointValues(grasp_q) else: print("Cannot find valid config") robot.arm.hand.Close() robot.arm.Grab(block, inverse_matrix(trans_com_ee)) utils.wait_for_user( 'Just grasped. Going to move to grasp pose. Ready?') robot.arm.SetJointValues(grasp_q) # transformation of EE in world frame (at final block position) p_com_world_goal = np.add(p_com_world_goal, tower_offset) trans_com_world_goal = compose_matrix( translate=p_com_world_goal, angles=euler_from_quaternion(q_com_world_goal)) trans_ee_world_goal = concatenate_matrices( trans_com_world_goal, trans_com_ee) # should be the other way around # move block to goal pose goal_q = robot.arm.ComputeIK(trans_ee_world_goal) if goal_q is not None: utils.wait_for_user("Move to desired pose?") robot.arm.SetJointValues(goal_q) else: print("Cannot find valid config") # release utils.wait_for_user('Going to release. Ready?') robot.arm.hand.Open() robot.arm.Release(block) for _ in range(1000): p.stepSimulation() utils.wait_for_user("Done?") utils.disconnect() # remove urdf files from pb_robot for (urdf_filename, _, _, _) in csv_data: pb_robot_path = os.path.join(os.getcwd(), 'pb_robot/' + urdf_filename) os.remove(pb_robot_path)
def batch(indices, augmentMode=None): """ :param indices: List of indices into the HDF5 dataset to draw samples from :return: (image, label) """ f = h5py.File(input_file) images = f['images'] labels = f['labels'] return_imgs = np.zeros(img_shape + (2, )) while True: np.random.shuffle(indices) for i in indices: t1_image = np.asarray(images[i, ..., 0], dtype='float32') t2_image = np.asarray(images[i, ..., 1], dtype='float32') try: true_labels = labels[i, ..., 0] if augmentMode is not None: if 'flip' in augmentMode: # flip images if np.random.rand() > 0.5: t1_image = np.flip(t1_image, axis=0) t2_image = np.flip(t2_image, axis=0) true_labels = np.flip(true_labels, axis=0) if 'affine' in augmentMode: if np.random.rand() > 0.5: scale = 1 + (np.random.rand(3) - 0.5) * 0.1 # up to 5% scale else: scale = None if np.random.rand() > 0.5: shear = (np.random.rand(3) - 0.5) * 0.2 # sheer of up to 10% else: shear = None if np.random.rand() > 0.5: angles = ( np.random.rand(3) - 0.5 ) * 0.1 * 2 * math.pi # rotation up to 5 degrees else: angles = None trans_mat = t.compose_matrix(scale=scale, shear=shear, angles=angles) trans_mat = trans_mat[0:-1, 0:-1] t1_image = affine_transform(t1_image, trans_mat, cval=10) t2_image = affine_transform(t2_image, trans_mat, cval=10) # ratio_img = affine_transform(ratio_img, trans_mat, cval=10) true_labels = affine_transform( true_labels, trans_mat, order=0, cval=10) # nearest neighbour for labels return_imgs[..., 0] = t1_image return_imgs[..., 1] = t2_image label = to_categorical( np.reshape(true_labels, true_labels.shape + (1, ))) yield (return_imgs[np.newaxis, ...], label[np.newaxis, ...]) except ValueError: print('some sort of value error occurred') # print(images[i, :, :, 80:-48][np.newaxis, ...].shape) yield (return_imgs[np.newaxis, ...])
def main(): SHOW_AXES = True AFFINE_IMG = True NO_SCALE = True n_tracts = 240 # n_tracts = 24 # n_threads = 2*psutil.cpu_count() img_shift = 256 # 255 data_dir = os.environ.get('OneDrive') + r'\data\dti_navigation\baran\anat_reg_improve_20200609' data_dir = data_dir.encode('utf-8') # FOD_path = 'Baran_FOD.nii' # trk_path = os.path.join(data_dir, FOD_path) # data_dir = b'C:\Users\deoliv1\OneDrive\data\dti' stl_path = b'wm_orig_smooth_world.stl' brain_path = os.path.join(data_dir, stl_path) # data_dir = b'C:\Users\deoliv1\OneDrive\data\dti' stl_path = b'wm_2.stl' brain_simnibs_path = os.path.join(data_dir, stl_path) stl_path = b'wm.stl' brain_inv_path = os.path.join(data_dir, stl_path) nii_path = b'Baran_FOD.nii' trk_path = os.path.join(data_dir, nii_path) nii_path = b'Baran_T1_inFODspace.nii' img_path = os.path.join(data_dir, nii_path) nii_path = b'Baran_trekkerACTlabels_inFODspace.nii' act_path = os.path.join(data_dir, nii_path) stl_path = b'magstim_fig8_coil.stl' coil_path = os.path.join(data_dir, stl_path) imagedata = nb.squeeze_image(nb.load(img_path.decode('utf-8'))) imagedata = nb.as_closest_canonical(imagedata) imagedata.update_header() pix_dim = imagedata.header.get_zooms() img_shape = imagedata.header.get_data_shape() act_data = nb.squeeze_image(nb.load(act_path.decode('utf-8'))) act_data = nb.as_closest_canonical(act_data) act_data.update_header() act_data_arr = act_data.get_fdata() # print(imagedata.header) print("pix_dim: {}, img_shape: {}".format(pix_dim, img_shape)) if AFFINE_IMG: affine = imagedata.affine if NO_SCALE: scale, shear, angs, trans, persp = tf.decompose_matrix(imagedata.affine) affine = tf.compose_matrix(scale=None, shear=shear, angles=angs, translate=trans, perspective=persp) else: affine = np.identity(4) print("affine: {0}\n".format(affine)) # Create a rendering window and renderer ren = vtk.vtkRenderer() ren_win = vtk.vtkRenderWindow() ren_win.AddRenderer(ren) ren_win.SetSize(800, 800) # Create a renderwindowinteractor iren = vtk.vtkRenderWindowInteractor() iren.SetRenderWindow(ren_win) minFODamp = np.arange(0.01, 0.11, 0.01) dataSupportExponent = np.arange(0.1, 1.1, 0.1) # COMBINATION 1 # tracker = minFODamp(0.01) # tracker = dataSupportExponent(0.1) # COMBINATION "n" # tracker = minFODamp(0.01 * n) # tracker = dataSupportExponent(0.1 * n) start_time = time.time() trekker_cfg = {'seed_max': 1, 'step_size': 0.1, 'min_fod': 0.1, 'probe_quality': 3, 'max_interval': 1, 'min_radius_curv': 0.8, 'probe_length': 0.4, 'write_interval': 50, 'numb_threads': '', 'max_lenth': 200, 'min_lenth': 20, 'max_sampling_step': 100} tracker = Trekker.initialize(trk_path) tracker, n_threads = dti.set_trekker_parameters(tracker, trekker_cfg) duration = time.time() - start_time print("Initialize Trekker: {:.2f} ms".format(1e3*duration)) repos = [0., -img_shift, 0., 0., 0., 0.] # brain_actor = load_stl(brain_inv_path, ren, opacity=1., colour=[1.0, 1.0, 1.0], replace=repos, user_matrix=np.identity(4)) # the one always been used brain_actor = load_stl(brain_simnibs_path, ren, opacity=1., colour=[1.0, 1.0, 1.0], replace=repos, user_matrix=np.linalg.inv(affine)) # bds = brain_actor.GetBounds() # print("Y length: {} --- Bounds: {}".format(bds[3] - bds[2], bds)) # invesalius surface # repos = [0., 0., 0., 0., 0., 0.] # brain_actor = load_stl(brain_inv_path, ren, opacity=.5, colour=[1.0, .5, .5], replace=repos, user_matrix=np.identity(4)) # repos = [0., 0., 0., 0., 0., 0.] # brain_actor_mri = load_stl(brain_path, ren, opacity=.1, colour=[0.0, 1.0, 0.0], replace=repos, user_matrix=np.linalg.inv(affine)) # bds = brain_actor_mri.GetBounds() # print("Y length: {} --- Bounds: {}".format(bds[3] - bds[2], bds)) # repos = [0., 256., 0., 0., 0., 0.] # brain_inv_actor = load_stl(brain_inv_path, ren, colour="SkinColor", opacity=0.5, replace=repos, user_matrix=np.linalg.inv(affine)) # brain_inv_actor = load_stl(brain_inv_path, ren, colour="SkinColor", opacity=.6, replace=repos) # bds = brain_inv_actor.GetBounds() # print("Reposed: Y length: {} --- Bounds: {}".format(bds[3] - bds[2], bds)) # Add axes to scene origin if SHOW_AXES: add_line(ren, [0, 0, 0], [150, 0, 0], color=[1.0, 0.0, 0.0]) add_line(ren, [0, 0, 0], [0, 150, 0], color=[0.0, 1.0, 0.0]) add_line(ren, [0, 0, 0], [0, 0, 150], color=[0.0, 0.0, 1.0]) # Show tracks repos_trk = [0., -img_shift, 0., 0., 0., 0.] # repos_trk = [0., 0., 0., 0., 0., 0.] matrix_vtk = vtk.vtkMatrix4x4() trans = np.identity(4) trans[1, -1] = repos_trk[1] final_matrix = np.linalg.inv(affine) @ trans print("final_matrix: {}".format(final_matrix)) for row in range(0, 4): for col in range(0, 4): matrix_vtk.SetElement(row, col, final_matrix[row, col]) root = vtk.vtkMultiBlockDataSet() # for i in range(10): # seed = np.array([[-8.49, -8.39, 2.5]]) # seed = np.array([[27.53, -77.37, 46.42]]) # from the invesalius exported fiducial markers you have to multiply the Y coordinate by -1 to # transform to the regular 3D invesalius space where coil location is saved fids_inv = np.array([[168.300, -126.600, 97.000], [9.000, -120.300, 93.700], [90.100, -33.500, 150.000]]) for n in range(3): fids_actor = add_marker(fids_inv[n, :], ren, [1., 0., 0.], radius=2) seed = np.array([[-25.66, -30.07, 54.91]]) coil_pos = [40.17, 152.28, 235.78, -18.22, -25.27, 64.99] m_coil = coil_transform_pos(coil_pos) repos = [0., 0., 0., 0., 0., 90.] coil_actor = load_stl(coil_path, ren, opacity=.6, replace=repos, colour=[1., 1., 1.], user_matrix=m_coil) # coil_actor = load_stl(coil_path, ren, opacity=.6, replace=repos, colour=[1., 1., 1.]) # create coil vectors vec_length = 75 print(m_coil.shape) p1 = m_coil[:-1, -1] print(p1) coil_dir = m_coil[:-1, 0] coil_face = m_coil[:-1, 1] p2_face = p1 + vec_length * coil_face p2_dir = p1 + vec_length * coil_dir coil_norm = np.cross(coil_dir, coil_face) p2_norm = p1 - vec_length * coil_norm add_line(ren, p1, p2_dir, color=[1.0, .0, .0]) add_line(ren, p1, p2_face, color=[.0, 1.0, .0]) add_line(ren, p1, p2_norm, color=[.0, .0, 1.0]) colours = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.], [1., .0, 1.], [.5, .5, 0.], [0., .5, .5], [1., 1., 0.], [1., .4, .0]] marker_actor = add_marker(p1, ren, colours[0], radius=1) # p1_change = n2m.coord_change(p1) p1_change = p1.copy() p1_change[1] = -p1_change[1] # p1_change[1] += img_shift marker_actor2 = add_marker(p1_change, ren, colours[1], radius=1) offset = 40 coil_norm = coil_norm/np.linalg.norm(coil_norm) coord_offset_nav = p1 - offset * coil_norm marker_actor_seed_nav = add_marker(coord_offset_nav, ren, colours[3], radius=1) coord_offset_mri = coord_offset_nav.copy() coord_offset_mri[1] += img_shift marker_actor_seed_nav = add_marker(coord_offset_mri, ren, colours[3], radius=1) coord_mri_label = [int(s) for s in coord_offset_mri] print("offset MRI: {}, and label: {}".format(coord_mri_label, act_data_arr[tuple(coord_mri_label)])) offset_list = 10 + np.arange(0, 31, 3) coord_offset_list = p1 - np.outer(offset_list, coil_norm) coord_offset_list += [0, img_shift, 0] coord_offset_list = coord_offset_list.astype(int).tolist() # for pt in coord_offset_list: # print(pt) # if act_data_arr[tuple(pt)] == 2: # cl = colours[5] # else: # cl = colours[4] # _ = add_marker(pt, ren, cl) x = np.arange(-4, 5, 2) y = np.arange(-4, 5, 2) z = 10 + np.arange(0, 31, 3) xv, yv, zv = np.meshgrid(x, y, - z) coord_grid = np.array([xv, yv, zv]) start_time = time.time() for p in range(coord_grid.shape[1]): for n in range(coord_grid.shape[2]): for m in range(coord_grid.shape[3]): pt = coord_grid[:, p, n, m] pt = np.append(pt, 1) pt_tr = m_coil @ pt[:, np.newaxis] pt_tr = np.squeeze(pt_tr[:3]).astype(int) + [0, img_shift, 0] pt_tr = tuple(pt_tr.tolist()) if act_data_arr[pt_tr] == 2: cl = colours[6] elif act_data_arr[pt_tr] == 1: cl = colours[7] else: cl = [1., 1., 1.] # print(act_data_arr[pt_tr]) _ = add_marker(pt_tr, ren, cl, radius=1) duration = time.time() - start_time print("Compute coil grid: {:.2f} ms".format(1e3*duration)) start_time = time.time() # create grid of points grid_number = x.shape[0]*y.shape[0]*z.shape[0] coord_grid = coord_grid.reshape([3, grid_number]).T # sort grid from distance to the origin/coil center coord_list = coord_grid[np.argsort(np.linalg.norm(coord_grid, axis=1)), :] # make the coordinates homogeneous coord_list_w = np.append(coord_list.T, np.ones([1, grid_number]), axis=0) # apply the coil transformation matrix coord_list_w_tr = m_coil @ coord_list_w # convert to int so coordinates can be used as indices in the MRI image space coord_list_w_tr = coord_list_w_tr[:3, :].T.astype(int) + np.array([[0, img_shift, 0]]) # extract the first occurrence of a specific label from the MRI image labs = act_data_arr[coord_list_w_tr[..., 0], coord_list_w_tr[..., 1], coord_list_w_tr[..., 2]] lab_first = np.argmax(labs == 1) if labs[lab_first] == 1: pt_found = coord_list_w_tr[lab_first, :] _ = add_marker(pt_found, ren, [0., 0., 1.], radius=1) # convert coordinate back to invesalius 3D space pt_found_inv = pt_found - np.array([0., img_shift, 0.]) # convert to world coordinate space to use as seed for fiber tracking pt_found_tr = np.append(pt_found, 1)[np.newaxis, :].T pt_found_tr = affine @ pt_found_tr pt_found_tr = pt_found_tr[:3, 0, np.newaxis].T duration = time.time() - start_time print("Compute coil grid fast: {:.2f} ms".format(1e3*duration)) # create tracts count_tracts = 0 start_time_all = time.time() # uncertain_params = list(zip(dataSupportExponent, minFODamp)) for n in range(0, round(n_tracts/n_threads)): # branch = dti.multi_block(tracker, seed, n_threads) # branch = dti.multi_block(tracker, pt_found_tr, n_threads) # rescale n so that there is no 0 opacity tracts n_param = (n % 10) + 1 branch = dti.multi_block_uncertainty(tracker, pt_found_tr, n_threads, n_param) count_tracts += branch.GetNumberOfBlocks() # start_time = time.time() # root = dti.tracts_root(out_list, root, n) root.SetBlock(n, branch) # duration = time.time() - start_time # print("Compute root {}: {:.2f} ms".format(n, 1e3*duration)) duration = time.time() - start_time_all print("Compute multi {}: {:.2f} ms".format(n, 1e3*duration)) print("Number computed tracts {}".format(count_tracts)) print("Number computed branches {}".format(root.GetNumberOfBlocks())) start_time = time.time() tracts_actor = dti.compute_actor(root, matrix_vtk) duration = time.time() - start_time print("Compute actor: {:.2f} ms".format(1e3*duration)) # Assign actor to the renderer # ren.AddActor(brain_actor) # ren.AddActor(brain_inv_actor) # ren.AddActor(coil_actor) start_time = time.time() ren.AddActor(tracts_actor) duration = time.time() - start_time print("Add actor: {:.2f} ms".format(1e3*duration)) # ren.AddActor(brain_actor_mri) planex, planey, planez = raw_image(act_path, ren) planex.SetInteractor(iren) planex.On() planey.SetInteractor(iren) planey.On() planez.SetInteractor(iren) planez.On() _ = add_marker(np.squeeze(seed).tolist(), ren, [0., 1., 0.], radius=1) _ = add_marker(np.squeeze(pt_found_tr).tolist(), ren, [1., 0., 0.], radius=1) _ = add_marker(pt_found_inv, ren, [1., 1., 0.], radius=1) # Enable user interface interactor iren.Initialize() ren_win.Render() iren.Start()
def sequential_to_rotating_radians(rvector): rmatrix = compose_matrix(angles=rvector, angle_order="sxyz") return euler_from_matrix(rmatrix[:3, :3], axes="rxyz")
def outputToAffineMatrix(self, output): translation = output[0:3] * self.max_translate euler = self.so3_to_euler_angles(output[3:6]) return trf.compose_matrix(angles=euler * np.pi/180, translate=translation)
def __init__(self, **kwargs): super().__init__(**kwargs) self.back_color = (0.3, 0.5, 0.8, 1) #(1,1,1, 1) self.is_decimated = False self.debug = False self.use_avg_vertices = False self.use_face_area_correction = False self.show_lines = False self.indexed_output = True self.is_animated = False self.x_angle = 0 self.model_matrix = tuple( transf.compose_matrix(scale=(1, 1, 1), angles=(0, np.pi / 1, 0)).ravel()) self.vertices, self.indices = load_model("link") print("Base Mesh: Vertices=" + "{:d}".format(len(self.vertices)) + " Triangles=" + "{:d}".format(len(self.indices))) print(self.vertices.shape) self.bbox = utility.bounding_box(points=self.vertices[:, :3]) self.tri_prog = self.ctx.program( vertex_shader=open("shaders/basic.vert", "r").read(), geometry_shader=open("shaders/basic.geom", "r").read(), fragment_shader=open("shaders/shader.frag", "r").read()) self.line_prog = self.ctx.program( vertex_shader=open("shaders/basic.vert", "r").read(), fragment_shader=open("shaders/basic.frag", "r").read()) #self.prog["width"].value = self.wnd.width #self.prog["height"].value = self.wnd.height self.tri_prog["bbox.min"] = self.bbox[0] self.tri_prog["bbox.max"] = self.bbox[1] self.tri_prog[ 'model'].value = self.model_matrix # (np.pi/4, np.pi/4, 0)).ravel()) self.tri_prog['view'].value = tuple(transf.identity_matrix().ravel()) self.tri_prog['proj'].value = tuple(transf.identity_matrix().ravel()) self.tri_prog["in_color"].value = (0.9, 0.9, 0.3, 1.0) self.line_prog["bbox.min"] = self.bbox[0] self.line_prog["bbox.max"] = self.bbox[1] self.line_prog['model'].value = self.model_matrix self.line_prog['view'].value = tuple(transf.identity_matrix().ravel()) self.line_prog['proj'].value = tuple(transf.identity_matrix().ravel()) self.line_prog["in_color"].value = (0.7, 0.2, 0.3, 1.0) self.vbo = self.ctx.buffer(self.vertices[:, :3].copy(order="C")) self.index_buffer = self.ctx.buffer( self.indices[:, :3].copy(order="C")) #print(self.vbo) self.tri_vao_base = self.ctx.vertex_array(self.tri_prog, [ (self.vbo, '3f', 'inVert'), ], self.index_buffer) self.line_vao_base = self.ctx.vertex_array(self.line_prog, [ (self.vbo, '3f', 'inVert'), ], self.index_buffer) self.current_tri_vao = self.tri_vao_base self.current_line_vao = self.line_vao_base self.shader_constants = { "NUM_TRIS": len(self.indices), "X": 1, "Y": 1, "Z": 1 } self.resolution = 25 self.num_clusters = self.resolution**3 self.float_to_int_scaling_factor = 2**13 self.image_shape = (self.num_clusters, 14) _, self.vertex_cluster_ids = get_vertex_cell_indices_ids( vertices=self.vertices[:, :3], resolution=self.resolution) self.vertex_cluster_ids = np.array(self.vertex_cluster_ids, dtype=np.int32) self.compute_prog1 = self.ctx.compute_shader( source("shaders/firstpass.comp", self.shader_constants)) self.compute_prog2 = self.ctx.compute_shader( source("shaders/secondpass.comp", self.shader_constants)) self.compute_prog3 = self.ctx.compute_shader( source("shaders/thirdpass.comp", self.shader_constants)) print("\tCompiled all 3 compute shaders!") self.vertex_buffer = self.ctx.buffer( self.vertices.astype("f4").tobytes()) self.vertex_buffer.bind_to_storage_buffer(binding=0) self.index_buffer = self.ctx.buffer( self.indices.astype("i4").tobytes()) self.index_buffer.bind_to_storage_buffer(binding=1) self.cluster_id_buffer = self.ctx.buffer(self.vertex_cluster_ids) self.cluster_id_buffer.bind_to_storage_buffer(binding=2) self.cluster_quadric_map_int = self.ctx.texture(size=self.image_shape, components=1, dtype="i4") self.cluster_quadric_map_int.bind_to_image(3, read=True, write=True) self.cluster_vertex_positions = self.ctx.texture( size=(self.num_clusters, 1), components=4, data=None, dtype="f4") self.cluster_vertex_positions.bind_to_image(4, read=False, write=True) self.output_indices = self.ctx.texture(size=(len(self.indices), 1), components=4, dtype="i4") self.output_indices.bind_to_image(5, read=False, write=True) self.output_tri_verts = self.ctx.texture(size=(self.num_clusters, 1), components=4, dtype="f4") self.output_tri_verts.bind_to_image(6, read=False, write=True) print("Finished Decimation Program Setup!") self.decimate_mesh() # VAOs set in decimate_mesh() if self.debug: self.debug_dump()
def matrix_from_xyzrpy(translate, rpy): return np.dot(tfm.compose_matrix(translate=translate), tfm.euler_matrix(*rpy)).tolist()