''' Script to visualize the surgical tools trials all transformed to the image (model) coordinate system)''' import tdsmParserMultis import os import ConfigParser import numpy as np from sys import stderr import math from mayavi import mlab import stl from mayavi.modules.text import Text # To access any VTK object, we use 'tvtk', which is a Python wrapping of # VTK replacing C++ setters and getters by Python properties and # converting numpy arrays to VTK arrays when setting data. from tvtk.tools import visual from tvtk.api import tvtk from tvtk.common import configure_input from traits.api import on_trait_change def Arrow_From_A_to_B(x1, y1, z1, x2, y2, z2, c): ar1=visual.arrow(x=x1, y=y1, z=z1, color = c) ar1.length_cone=0.4 arrow_length=np.sqrt((x2-x1)**2+(y2-y1)**2+(z2-z1)**2) ar1.actor.scale=[arrow_length, arrow_length, arrow_length] ar1.pos = ar1.pos/arrow_length ar1.axis = [x2-x1, y2-y1, z2-z1] return ar1 def get_transformation_matrix(q1, q2, q3, q4, q5, q6): ''' Transform from optotrak global coordinates to optotrak position sensor coordinates''' T = np.zeros((4, 4)) T[0, 0] = math.cos(q6) * math.cos(q5) T[1, 0] = math.sin(q6) * math.cos(q5) T[2, 0] = -math.sin(q5) T[0, 1] = math.cos(q6) * math.sin(q5) * math.sin(q4) - math.sin(q6) * math.cos(q4) T[1, 1] = math.sin(q6) * math.sin(q5) * math.sin(q4) + math.cos(q6) * math.cos(q4) T[2, 1] = math.cos(q5) * math.sin(q4) T[0, 2] = math.cos(q6) * math.sin(q5) * math.cos(q4) + math.sin(q6) * math.sin(q4) T[1, 2] = math.sin(q6) * math.sin(q5) * math.cos(q4) - math.cos(q6) * math.sin(q4) T[2, 2] = math.cos(q5) * math.cos(q4) T[0, 3] = q1 T[1, 3] = q2 T[2, 3] = q3 T[3, 3] = 1 return T def convertTOarray(data): data = data.replace("", '') data = data.split(" ") idx = 0 for d in data: try: float(d) break except: idx +=1 corrected_list = data[idx:len(data)] corrected_list[-1] = corrected_list[-1][0:-1] data_float = map(float, corrected_list) return np.asarray(data_float) def fit_hypersphere(data, method="Hyper"): """ FitHypersphere.py fit_hypersphere(collection of tuples or lists of real numbers) will return a hypersphere of the same dimension as the tuples: (radius, (center)) using the Hyper (hyperaccurate) algorithm of Ali Al-Sharadqah and Nikolai Chernov Error analysis for circle fitting algorithms Electronic Journal of Statistics Vol. 3 (2009) 886-911 DOI: 10.1214/09-EJS419 generalized to n dimensions Mon Apr 23 04:08:05 PDT 2012 Kevin Karplus Note: this version using SVD works with Hyper, Pratt, and Taubin methods. If you are not familiar with them, Hyper is probably your best choice. Creative Commons Attribution-ShareAlike 3.0 Unported License. http://creativecommons.org/licenses/by-sa/3.0/ """ """returns a hypersphere of the same dimension as the collection of input tuples (radius, (center)) Methods available for fitting are "algebraic" fitting methods Hyper Al-Sharadqah and Chernov's Hyperfit algorithm Pratt Vaughn Pratt's algorithm Taubin G. Taubin's algorithm The following methods, though very similar, are not implemented yet, because the contraint matrix N would be singular, and so the N_inv computation is not doable. Kasa Kasa's algorithm """ num_points = len(data) # print >>stderr, "DEBUG: num_points=", num_points if num_points == 0: return (0, None) if num_points == 1: return (0, data[0]) dimen = len(data[0]) # dimensionality of hypersphere # print >>stderr, "DEBUG: dimen=", dimen if num_points < dimen + 1: raise ValueError( \ "Error: fit_hypersphere needs at least {} points to fit {}-dimensional sphere, but only given {}".format( dimen + 1, dimen, num_points)) # central dimen columns of matrix (data - centroid) central = np.matrix(data, dtype=float) # copy the data centroid = np.mean(central, axis=0) for row in central: row -= centroid # print >>stderr, "DEBUG: central=", repr(central) # squared magnitude for each centered point, as a column vector square_mag = [sum(a * a for a in row.flat) for row in central] square_mag = np.matrix(square_mag).transpose() # print >>stderr, "DEBUG: square_mag=", square_mag if method == "Taubin": # matrix of normalized squared magnitudes, data mean_square = square_mag.mean() data_Z = np.bmat([[(square_mag - mean_square) / (2 * math.sqrt(mean_square)), central]]) # print >> stderr, "DEBUG: data_Z=",data_Z u, s, v = np.linalg.svd(data_Z, full_matrices=False) param_vect = v[-1, :] params = [x for x in np.asarray(param_vect)[0]] # convert from (dimen+1) x 1 matrix to list params[0] /= 2 * math.sqrt(mean_square) params.append(-mean_square * params[0]) params = np.array(params) else: # matrix of squared magnitudes, data, 1s data_Z = np.bmat([[square_mag, central, np.ones((num_points, 1))]]) # print >> stderr, "DEBUG: data_Z=",data_Z # SVD of data_Z # Note: numpy's linalg.svd returns data_Z = u * s * v # not u*s*v.H as the Release 1.4.1 documentation claims. # Newer documentation is correct. u, s, v = np.linalg.svd(data_Z, full_matrices=False) # print >>stderr, "DEBUG: u=",repr(u) # print >>stderr, "DEBUG: s=",repr(s) # print >>stderr, "DEBUG: v=",repr(v) # print >>stderr, "DEBUG: v.I=",repr(v.I) if s[-1] / s[0] < 1e-12: # singular case # param_vect as (dimen+2) x 1 matrix param_vect = v[-1, :] # Note: I get last ROW of v, while Chernov claims last COLUMN, # because of difference in definition of SVD for MATLAB and numpy # print >> stderr, "DEBUG: singular, param_vect=", repr(param_vect) # print >> stderr, "DEBUG: data_Z*V=", repr(data_Z*v) # print >> stderr, "DEBUG: data_Z*VI=", repr(data_Z*v.I) # print >> stderr, "DEBUG: data_Z*A=", repr(data_Z*v[:,-1]) else: Y = v.H * np.diag(s) * v Y_inv = v.H * np.diag([1. / x for x in s]) * v # print >>stderr, "DEBUG: Y=",repr(Y) # print >>stderr, "DEBUG: Y.I=",repr(Y.I), "\nY_inv=",repr(Y_inv) # Ninv is the inverse of the constraint matrix, after centroid has been removed Ninv = np.asmatrix(np.identity(dimen + 2, dtype=float)) if method == "Hyper": Ninv[0, 0] = 0 Ninv[0, -1] = 0.5 Ninv[-1, 0] = 0.5 Ninv[-1, -1] = -2 * square_mag.mean() elif method == "Pratt": Ninv[0, 0] = 0 Ninv[0, -1] = -0.5 Ninv[-1, 0] = -0.5 Ninv[-1, -1] = 0 else: raise ValueError("Error: unknown method: {} should be 'Hyper', 'Pratt', or 'Taubin'") # print >> stderr, "DEBUG: Ninv=", repr(Ninv) # get the eigenvector for the smallest positive eigenvalue matrix_for_eigen = Y * Ninv * Y # print >> stderr, "DEBUG: {} matrix_for_eigen=\n{}".format(method, repr(matrix_for_eigen)) eigen_vals, eigen_vects = np.linalg.eigh(matrix_for_eigen) # print >> stderr, "DEBUG: eigen_vals=", repr(eigen_vals) # print >> stderr, "DEBUG: eigen_vects=", repr(eigen_vects) positives = [x for x in eigen_vals if x > 0] if len(positives) + 1 != len(eigen_vals): # raise ValueError("Error: for method {} exactly one eigenvalue should be negative: {}".format(method,eigen_vals)) print>> stderr, "Warning: for method {} exactly one eigenvalue should be negative: {}".format(method, eigen_vals) smallest_positive = min(positives) # print >> stderr, "DEBUG: smallest_positive=", smallest_positive # chosen eigenvector as 1 x (dimen+2) matrix A_colvect = eigen_vects[:, list(eigen_vals).index(smallest_positive)] # print >> stderr, "DEBUG: A_colvect=", repr(A_colvect) # now have to multiply by Y inverse param_vect = (Y_inv * A_colvect).transpose() # print >> stderr, "DEBUG: nonsingular, param_vect=", repr(param_vect) params = np.asarray(param_vect)[0] # convert from (dimen+2) x 1 matrix to array of (dimen+2) # print >> stderr, "DEBUG: params=", repr(params) radius = 0.5 * math.sqrt(sum(a * a for a in params[1:-1]) - 4 * params[0] * params[-1]) / abs(params[0]) center = -0.5 * params[1:-1] / params[0] # y print >> stderr, "DEBUG: center=", repr(center), "centroid=", repr(centroid) center += np.asarray(centroid)[0] return (radius, center) def transform_sphere_points(B1m, B1mr): ''' Function to extract the centers of each registration marker in the respective bone Optotrak sensor coordinate system''' # Within this data set, there are 30 points, each with an x,y,and z coordinate # next we want to convert this data set into an array. The units for the axis # are all in m. # But first need to get rid of the quotations at the beginning and end of string. B1m = B1m.replace('"', '') B1msplit = B1m.split(" ") # Get rid of first two cells, and convert string into float corrected_list = B1msplit[2:92] corrected_floatlist = map(float, corrected_list) ## matrix of the 30 points B1mCoord = np.asarray(corrected_floatlist) B1mCoord = B1mCoord.reshape(30, -1) # Within this data set, there are 30 points, each with an x,y,z,roll, pitch and # yaw coordinate next we want to convert this data set into an array. The # units for the x,y, and z axis are in m and the roll, pitch, and yaw axis # are in rad. # But first need to get rid of the quotations at the beginning and end of string. B1mr = B1mr.replace('"', '') B1mrsplit = B1mr.split(" ") # Get rid of first two cells, and convert string into float corrected_list = B1mrsplit[2:182] corrected_floatlist = map(float, corrected_list) ## matrix of the 30 points B1mrCoord = np.asarray(corrected_floatlist) B1mrCoord = B1mrCoord.reshape(30, -1) ## Define coordinate transformations of data P1 = np.ones((4, 1)) Coord1 = np.zeros((30, 3)) for i in range(0, 30): q1 = B1mrCoord[i, 0] q2 = B1mrCoord[i, 1] q3 = B1mrCoord[i, 2] q4 = B1mrCoord[i, 3] q5 = B1mrCoord[i, 4] q6 = B1mrCoord[i, 5] T1 = get_transformation_matrix(q1, q2, q3, q4, q5, q6) P1[0, 0] = B1mCoord[i, 0] P1[1, 0] = B1mCoord[i, 1] P1[2, 0] = B1mCoord[i, 2] invT1 = np.linalg.inv(T1) A = np.dot(invT1, P1) # Transform to bone Optotrak sensor coordinate system Coord1[i, 0] = A[0, 0] Coord1[i, 1] = A[1, 0] Coord1[i, 2] = A[2, 0] ACoord1 = Coord1[0:10, 0:3] BCoord1 = Coord1[10:20, 0:3] CCoord1 = Coord1[20:30, 0:3] # Sphere fit for Rigid Body Collected Points (m), all three spheres. Set to NAN if points were not digitized. NAN = float('nan') try: B1mSphereA = fit_hypersphere(ACoord1, method="Pratt") except: B1mSphereA = [(0), (NAN, NAN, NAN)] try: B1mSphereB = fit_hypersphere(BCoord1, method="Pratt") except: B1mSphereB = [(0), (NAN, NAN, NAN)] try: B1mSphereC = fit_hypersphere(CCoord1, method="Pratt") except: B1mSphereC = [(0), (NAN, NAN, NAN)] return B1mSphereA, B1mSphereB, B1mSphereC def transform_digitized_points(B1m, B1mr, N): ''' Function to extract the centers of each registration marker in the respective bone Optotrak sensor coordinate system''' # Within this data set, there are 30 points, each with an x,y,and z coordinate # next we want to convert this data set into an array. The units for the axis # are all in m. # But first need to get rid of the quotations at the beginning and end of string. B1m = B1m.replace('"', '') B1msplit = B1m.split(" ") # Get rid of first two cells, and convert string into float corrected_list = B1msplit[2:3*N + 2] corrected_floatlist = map(float, corrected_list) B1mCoord = np.asarray(corrected_floatlist) B1mCoord = B1mCoord.reshape(N, -1) # Within this data set, there are 30 points, each with an x,y,z,roll, pitch and # yaw coordinate next we want to convert this data set into an array. The # units for the x,y, and z axis are in m and the roll, pitch, and yaw axis # are in rad. # But first need to get rid of the quotations at the beginning and end of string. B1mr = B1mr.replace('"', '') B1mrsplit = B1mr.split(" ") # Get rid of first two cells, and convert string into float corrected_list = B1mrsplit[2:6*N + 2] corrected_floatlist = map(float, corrected_list) B1mrCoord = np.asarray(corrected_floatlist) B1mrCoord = B1mrCoord.reshape(N, -1) ## Define coordinate transformations of data P1 = np.ones((4, 1)) Coord1 = np.zeros((N, 3)) for i in range(0, N): q1 = B1mrCoord[i, 0] q2 = B1mrCoord[i, 1] q3 = B1mrCoord[i, 2] q4 = B1mrCoord[i, 3] q5 = B1mrCoord[i, 4] q6 = B1mrCoord[i, 5] T1 = get_transformation_matrix(q1, q2, q3, q4, q5, q6) P1[0, 0] = B1mCoord[i, 0] P1[1, 0] = B1mCoord[i, 1] P1[2, 0] = B1mCoord[i, 2] invT1 = np.linalg.inv(T1) A = np.dot(invT1, P1) # Transform to bone Optotrak sensor coordinate system Coord1[i, 0] = A[0, 0] Coord1[i, 1] = A[1, 0] Coord1[i, 2] = A[2, 0] return Coord1 def main(dir, segment, file): # Assign seg and bone parameters for later calculations if segment == 'UpperLeg': seg = 'UL_' bone = 'Femur' markers = ["F1", "F2", "F3", "F4", "F5", "F6"] v = mlab.figure() # Create Mayavi figure visual.set_viewer(v) dir_data = os.path.join(dir, 'Data') data = tdsmParserMultis.parseTDMSfile(os.path.join(dir_data, file)) tools = ['Forceps', 'Probe', 'Scalpel', 'Retractor'] time = np.array(data[u'Time'][u'Time']) actors = [None] * len(time) config = ConfigParser.RawConfigParser() if not config.read(os.path.join(os.path.join(dir, 'Configuration'), file[0:-5]+'_State.cfg')): raise IOError, "Cannot load configuration file... Check path." # Get transformation matrix of CAD points and tool coordinate system for ALL tools TOOLS = ['Forceps Left', 'Forceps Right', 'Retractor', 'Scalpel', 'Probe'] T_CAD_TOS_dict = {} for t in TOOLS: CAD_data = np.genfromtxt(os.path.join('divot_points_in_world', t.replace(" ", "")+'Points2.dat.txt'), delimiter='\t', skip_header=1, usecols=(1,2,3)) CAD_data_rNames = np.genfromtxt(os.path.join('divot_points_in_world', t.replace(" ", "")+'Points2.dat.txt'), delimiter='\t', skip_header=1, usecols=(0), dtype='S') CAD_pts = CAD_data[[i for i, s in enumerate(CAD_data_rNames) if 'Handle' in s and 'Div' in s]] B1m = config.get(t+' LC2REF', 'Collected Points Rigid Body 2 (m)') # Proximal Digitized points (global) B1mr = config.get(t+' LC2REF', 'Collected Points Rigid Body 2 Position Sensor (m,rad)') # Position of bone Optotrak Sensor digitized_points = transform_digitized_points(B1m, B1mr, len(CAD_pts)) # digitized points in tool optotrak sensor coordinate system # Create transformation matrix using CAD points and digitized points (SVD method) x = np.array(digitized_points) # world points y = np.array(CAD_pts) / 1000 # CAD points x_bar = np.mean(x, axis=0) y_bar = np.mean(y, axis=0) A = (x - x_bar).transpose() B = (y - y_bar).transpose() C = np.dot(B, A.transpose()) P, D, QT = np.linalg.svd(C, full_matrices=True) Rot = np.dot(np.dot(P, np.diag([1, 1, np.linalg.det(np.dot(P, QT))])), QT) d = y_bar.transpose() - np.dot(Rot, x_bar.transpose()) # Define transformation matrix Tool Optotrak Sensor in CAD CS T_CAD_TOS = np.zeros((4, 4)) T_CAD_TOS[0:3, 0:3] = Rot T_CAD_TOS[3, 3] = 1 T_CAD_TOS[0:3, 3] = d T_CAD_TOS_dict[t] = T_CAD_TOS for tool in tools: print tool if tool == 'Forceps': tool_LC = 'Forceps Right LC2REF' tool_pos = 'Right Forcep Pos' tool_pos_config = 'Forceps Right' elif tool == 'Probe': tool_LC = tool + ' LC2REF' tool_pos = 'Scalpel Pos' tool_pos_config = tool else: tool_LC = tool + ' LC2REF' tool_pos = tool + ' Pos' tool_pos_config = tool T_CAD_TOS = T_CAD_TOS_dict[tool_pos_config] # If not already defined for this subject, find the transformation between the bone optotrack sensor coordinate system and image coordinate system. if 'T_I_BOS' not in locals(): B1m = config.get(bone + '-Sphere Positions', 'Collected Points Rigid Body 1 (m)') # Proximal Digitized points (global) B1mr = config.get(bone + '-Sphere Positions', 'Collected Points Rigid Body 1 Position Sensor (m,rad)') # Position of bone Optotrak Sensor B2m = config.get(bone + '-Sphere Positions', 'Collected Points Rigid Body 2 (m)') # Distal Digitized points (global) B2mr = config.get(bone + '-Sphere Positions', 'Collected Points Rigid Body 2 Position Sensor (m,rad)') # Position of bone Optotrak Sensor # Get digitized sphere centers (bone sensor coordinate system) spheres_prox = transform_sphere_points(B1m, B1mr) spheres_dist = transform_sphere_points(B2m, B2mr) sphere_fit_rm = np.array(map(list, zip(*(spheres_prox+spheres_dist)))[1]) sphere_fit_rm = sphere_fit_rm[~np.isnan(sphere_fit_rm)].reshape((-1,3)) # Get STL for tool filename2 = os.path.join('ToolSTLs', tool.lower() + '.stl') R = [time[0], time[-1]] idxs = np.where(np.logical_and(time >= R[0], time <= R[1])) time = time[idxs[0]] # Get the state data for tool position w.r.t. the bone coordinate system Offset = config.get(tool_pos_config, 'position offset (m,rad)') Offset = convertTOarray(Offset) B_pos_st = data[u'State.' + tool_pos_config] x_state = B_pos_st[u'' + tool_pos_config + '_x'][idxs[0]] / 1000 + Offset[0] y_state = B_pos_st[u'' + tool_pos_config + '_y'][idxs[0]] / 1000 + Offset[1] z_state = B_pos_st[u'' + tool_pos_config + '_z'][idxs[0]] / 1000 + Offset[2] r_state = np.radians(B_pos_st[u'' + tool_pos_config + '_roll'])[idxs[0]] + Offset[3] p_state = np.radians(B_pos_st[u'' + tool_pos_config + '_pitch'])[idxs[0]] + Offset[4] w_state = np.radians(B_pos_st[u'' + tool_pos_config + '_yaw'])[idxs[0]] + Offset[5] # Transformation matrix from bone Optotrak sensor coordinate system to bone coordinate system T_BOS_B = config.get(tool_pos_config, 't_sensor1_rb1') T_BOS_B = convertTOarray(T_BOS_B).reshape(4, -1) # Transformation matrix from tool Optotrak sensor coordinate system to tool coordinate system T_TOS_T = config.get(tool_pos_config, 't_sensor2_rb2') T_TOS_T = convertTOarray(T_TOS_T).reshape(4, -1) for i, s in enumerate(spheres_prox): if ~np.isnan(s[1]).any(): sphere = np.ones(4) sphere[0:3] = s[1] sphere = np.dot(np.linalg.inv(T_BOS_B), sphere) mlab.points3d(sphere[0] * 1000, sphere[1] * 1000, sphere[2] * 1000, 1, scale_factor=20, figure=v, opacity=0.1, color=(1, 0, 0), mode='sphere') for i, s in enumerate(spheres_dist): if ~np.isnan(s[1]).any(): sphere = np.ones(4) sphere[0:3] = s[1] sphere = np.dot(np.linalg.inv(T_BOS_B), sphere) mlab.points3d(sphere[0] * 1000, sphere[1] * 1000, sphere[2] * 1000, 1, scale_factor=20, figure=v, opacity=0.1, color=(1, 0, 0), mode='sphere') mlab.points3d(0, 0, 0, 1, scale_factor=5, figure=v, opacity=1, color=(1, 1, 0), mode='sphere') # Add coordinate system arrows to Mayavi window visual.set_viewer(v) Arrow_From_A_to_B(0,0,0,0,0,50, (0,0,1)) Arrow_From_A_to_B(0, 0, 0, 0, 50, 0, (0,1,0)) Arrow_From_A_to_B(0, 0, 0, 50, 0, 0, (1,0,0)) # x_data = [] for t in range(len(time)): T_B_T = get_transformation_matrix(x_state[t], y_state[t], z_state[t], r_state[t], p_state[t], w_state[t]) T_B_CAD_st = np.dot(np.dot(T_B_T, np.linalg.inv(T_TOS_T)), np.linalg.inv(T_CAD_TOS)) # Ultrasound probe tip coordinates in the image coordinate system A = T_B_CAD_st # Define what coordinate system to plot everything in stl_file = stl.Mesh.from_file(filename2, calculate_normals=True) ################################ # Add tool stl from state data! ################################ A[0][3] = A[0][3] * 1000 A[1][3] = A[1][3] * 1000 A[2][3] = A[2][3] * 1000 try: stl_file.transform(A) stl_file.save(filename2[0:-4] + '_1.stl') filename3 = filename2[0:-4] + '_1.stl' reader2 = tvtk.STLReader() reader2.file_name = file_name = filename3 reader2.update() mapper2 = tvtk.PolyDataMapper() configure_input(mapper2, reader2.output) # Add ultrasound probe to Mayavi scene prop = tvtk.Property(opacity=1, color = (1,0,0)) if actors[t]==None: actors[t] = [tvtk.Actor(mapper=mapper2, property=prop)] else: actors[t].append([tvtk.Actor(mapper=mapper2, property=prop)]) except: print "Missing data at time = {:0.3f} seconds".format(time[t]/1000.0) # time = np.delete(time, t) @mlab.show @mlab.animate(delay=20) @on_trait_change('scene.activated') def anim(): """Animate the b1 box.""" t = 0 engine = mlab.get_engine() scene = engine.current_scene timetext = Text() timetext.text = 'Time = %f sec' % (float(time[t]) / 1000.0) v.scene.add_actor(timetext.actor) # timetext = mlab.text(0.7, 0.01, 'Time = %f sec' % (float(time[t]) / 1000.0), figure = scene) while 1: if 'actor_old' in locals(): for a in actor_old: v.scene.remove_actor(a) for a in actors[t]: v.scene.add_actor(a) actor_old = actors[t] timetext.text = 'Time = %f sec' % (float(time[t]) / 1000.0) t = (t + 1) % len(time) yield anim() if __name__ == "__main__": dir = '/home/morrile2/Documents/MULTIS Data/MULTIS_surgtools/SMULTIS033-1' # Cadaver specimen folder segment = 'UpperLeg' # Change this to the desired segment tdmsFile = '003_SMULTIS033-1_SXX_CUT_SKN-1.tdms' main(dir, segment, tdmsFile)