import tdsmParserMultis
import os
import ConfigParser
import XMLparser
import numpy as np
from sys import stderr
import math
from mayavi import mlab
import stl
import tkFileDialog
import dicom
import peakutils
import xml.etree.ElementTree as ET
# 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.api import tvtk
from tvtk.common import configure_input
import ProbeCoordinates_2
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(" ")
corrected_list = data[2:len(data)]
corrected_list[-1] = corrected_list[-1][0:-1]
data_float = map(float, corrected_list)
return np.asarray(data_float)
def get_line(A, B):
length = math.sqrt((A[0]-B[0])**2+(A[1]-B[1])**2+(A[2]-B[2])**2)
vector = (A - B)
return float(length), vector[0], vector[1], vector[2]
def get_xyzrpw(T):
x = T[0,3]
y = T[1,3]
z = T[2,3]
w = math.atan2(T[1,0], T[0,0])
p = math.atan2(-T[2,0], math.sqrt(T[2,1]**2+T[2,2]**2))
r = math.atan2(T[2,1], T[2,2])
return x, y, z, r, p, w
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 findFrame(dT, frameTimeVector, initial_time):
"""Find the frame corresponding to the specified tdms time and return the adjusted tdms time to match the
selected frame"""
adjusted_time = dT + initial_time
for f in range(len(frameTimeVector)):
f += 1
frame_time = sum(frameTimeVector[0:f])
if adjusted_time <= frame_time:
timeDiff_up = frame_time - adjusted_time
timeDiff_low = adjusted_time - sum(frameTimeVector[0:f - 1])
if timeDiff_up < timeDiff_low:
frame_frame = f
readjusted_time_tdms = frame_time - dT
else:
frame_frame = f - 1
readjusted_time_tdms = sum(frameTimeVector[0:f - 1]) - dT
break
return frame_frame, int(readjusted_time_tdms)
def find_file(name, path):
for root, dirs, files in os.walk(path):
if name in files:
return os.path.join(root, name)
def createAverageFit(F, avgThres):
"""Filter the tdms normal force data"""
averagelist = []
for items in range(len(F)):
if F[items] != F[items - avgThres]:
num2avg = F[items:(items + avgThres)]
averagelist.append(np.average(num2avg))
else:
continue
averagelist = [averagelist[0]] * (avgThres / 2) + averagelist[0:-(avgThres) / 2]
return averagelist
def getFrames(dir, tdms, frameTimeVector):
"""Get the frames to be analyzed and save the data for those frames. Different for indentation and anatomical
trials. Indentation contains frames that start at indentation and go through the peak force of the indentation,
while anatomical analyzes all frames between start and end pulses from minimum to maximum force"""
# Find the xml file with delta_t
analysis_path = os.path.join(os.path.split(dir)[0], 'TimeSynchronization')
split_name = tdms
tail = split_name[0:-5] + '_dT.xml'
delta_t_file = find_file(tail, analysis_path)
# print(delta_t_file)
if delta_t_file == None:
return
else:
# Extract force information from TDMS file
data = tdsmParserMultis.parseTDMSfile(os.path.join(dir, tdms))
Fx = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Fx'])
Fy = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Fy'])
Fz = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Fz'])
Mx = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Mx'])
My = np.array(data[u'State.6-DOF Load'][u'6-DOF Load My'])
Mz = np.array(data[u'State.6-DOF Load'][u'6-DOF Load Mz'])
F_mag = []
for f in range(len(Fx)):
F_mag.append(math.sqrt(Fx[f] ** 2 + Fy[f] ** 2 + Fz[f] ** 2))
pulse = np.array(data[u'Sensor.Run Number Pulse Train'][u'Run Number Pulse Train'])
pulse = pulse[:]
Peaks = peakutils.indexes(pulse, thres=0.5 * max(pulse), min_dist=100)
doc1 = ET.parse(delta_t_file)
root1 = doc1.getroot()
loc1 = root1.find("Location")
dT_str = loc1.find("dT").text
dT = float(dT_str)
if tdms[-8:-7] == 'I':
indentation = True
anatomical = False
elif tdms[-8:-7] == 'A':
indentation = False
anatomical = True
# force_list = list(F_mag)
time = np.arange(len(F_mag))
if indentation:
# Indentation frames from start of indentation to peak force during indentation
# Find indentation start time in TDMS timeline , denoted by tdms b/c used for tdms
data_i = zip(time, Fx, Fy, Fz, Mx, My, Mz, F_mag)
force_list = createAverageFit(F_mag, 300)
time_max_tdms = force_list.index(max(force_list))
r_sq_old = 0
r_sq_diff = 1
i = 500
# print(max(force_list[0:230])-min(force_list[0:230]))
while r_sq_diff > .1:
if i > time_max_tdms:
time_start_tdms = 230
break
else:
x = np.arange(time_max_tdms - i, time_max_tdms, 1)
y = np.array(force_list[time_max_tdms - i:time_max_tdms])
r_sq_diff = -r_sq_old + (y[-1] - y[0])
# print(r_sq_diff)
r_sq_old = (y[-1] - y[0])
time_start_tdms = x[100]
i += 100
start_frame, start_frame_time_tdms = findFrame(dT, frameTimeVector, time_start_tdms)
max_frame, max_frame_time_tdms = findFrame(dT, frameTimeVector, time_max_tdms)
frame_lst_i = [start_frame]
data_i_final = [data_i[start_frame_time_tdms]]
tdmsTime = [start_frame_time_tdms]
for t in data_i[start_frame_time_tdms + 1:max_frame_time_tdms]:
inc_frame, inc_frame_time_tdms = findFrame(dT, frameTimeVector, t[0])
if inc_frame not in frame_lst_i:
frame_lst_i.append(inc_frame)
data_i_final.append(data_i[inc_frame_time_tdms])
tdmsTime.append(inc_frame_time_tdms)
frames = frame_lst_i
DATA = data_i_final
tdmsTimes = tdmsTime
elif anatomical:
# Anatomical frames from minimum to maximum normal force (Fx)
time_preIndent_tdms = Peaks[0] # 230 ms is location first pulse.
time_postIndent_tdms = Peaks[-1]
# Sort the data by the magnitude (F_mag) to get anatomical frame list from minimum to maximum
data_a = zip(time, Fx, Fy, Fz, Mx, My, Mz, F_mag)
data_a.sort(key=lambda t: abs(t[7]))
frame_lst_a = []
data_a_sort = []
tdmsTime = []
for t in data_a:
try:
min_frame, min_frame_time_tdms = findFrame(dT, frameTimeVector, t[0])
except:
continue
if min_frame not in frame_lst_a:
if min_frame_time_tdms > time_preIndent_tdms and min_frame_time_tdms < time_postIndent_tdms:
frame_lst_a.append(min_frame)
data_a_sort.append(data_a[min_frame_time_tdms])
tdmsTime.append(min_frame_time_tdms)
final_a_sort = zip(frame_lst_a, data_a_sort, tdmsTime)
final_a_sort.sort(key=lambda t: abs(t[1][7]))
# #Analyze the min, max, and middle force
frames = [final_a_sort[0][0], final_a_sort[int(len(final_a_sort) / 2)][0],
final_a_sort[len(final_a_sort) - 1][0]]
DATA = [final_a_sort[0][1], final_a_sort[int(len(final_a_sort) / 2)][1],
final_a_sort[len(final_a_sort) - 1][1]]
tdmsTimes = [final_a_sort[0][2], final_a_sort[int(len(final_a_sort) / 2)][2],
final_a_sort[len(final_a_sort) - 1][2]]
return frames, DATA, tdmsTimes
def get_stl_points(file):
reader = tvtk.STLReader()
reader.file_name = file
reader.update()
stl_points = reader.output.points.data.to_array()
return stl_points
def get_stl_dist(subj_path, modality, reg_ID, markers):
spheres = []
for m in markers:
if "N/A" not in m:
spheres.append(os.path.join(subj_path, 'Registration', reg_ID, "MarkerSTLs", modality, reg_ID + '_' + os.path.split(subj_path)[1]+'_'+m+'_'+modality[0:2]+'.stl'))
labels = []
for s in spheres:
filename = os.path.split(s)[1]
labels.append(filename[-6:-4])
sphere_fit = []
for i, s in enumerate(spheres):
r, pos = fit_hypersphere(get_stl_points(s))
sphere_fit.append([os.path.split(s)[1], r, pos])
center_dist = []
# print "STLS"
for p1 in range(len(sphere_fit)-1):
for p2 in range(p1+1,len(sphere_fit)):
d, x_a, y_a, z_a = get_line(sphere_fit[p1][2], sphere_fit[p2][2])
center_dist.append([sphere_fit[p1][0], sphere_fit[p2][0], d, x_a, y_a, z_a])
return center_dist, spheres, sphere_fit
def write_quality_check_doc(f, dir, segment, modality, sphere_fit, digitized_dist, STL_sphere_fit, img_dist):
f.write("Subject: " + os.path.split(dir)[1] + '\n')
f.write("Segment: " + segment + '\n')
f.write("Modality: " + modality + '\n')
f.write("\n")
f.write("Digitized Points\n")
f.write("\n")
f.write("\tSphere Fitting\n")
f.write("\tMarker\t\tRadius (mm)\tCoordinates [x,y,z] (mm)\n")
for l in sphere_fit:
if l[0] != 'N/A':
l[1] *= 1000
l[2]
f.write("\t" + l[0] + "\t\t%0.2f\t\t%0.4F, %0.4f, %0.4f\n" % (
float(l[1]), float(l[2][0]), float(l[2][1]), float(l[2][2])))
f.write("\n")
f.write("\tDistances between markers\n")
f.write("\tMarker 1\tMarker 2\tDistance (mm)\n")
for l in digitized_dist:
f.write("\t" + l[0] + "\t\t" + l[1] + "\t\t%0.2f\n" % ((l[2])))
f.write("\n")
f.write("Sphere Fitting - " + modality + " STL\n")
f.write("\n")
f.write("\tMarker\t\tRadius (mm)\tCoordinates [x,y,z] (mm)\n")
for l in STL_sphere_fit:
f.write("\t" + l[0][-9:-7] + "\t\t%0.2f\t\t%0.4F, %0.4f, %0.4f\n" % (
float(l[1]), float(l[2][0]), float(l[2][1]), float(l[2][2])))
f.write("\n")
f.write("\tDistances between markers\n")
f.write("\tMarker 1\tMarker 2\tDistance (mm)\n")
for l in img_dist:
f.write("\t" + l[0][-9:-7] + "\t\t" + l[1][-9:-7] + "\t\t%0.2f\n" % (l[2]))
diff = (np.array(img_dist).transpose()[2].astype(float) - np.array(digitized_dist).transpose()[2].astype(float))
diff_percent = (np.array(img_dist).transpose()[2].astype(float) - np.array(digitized_dist).transpose()[2].astype(
float)) / np.array(img_dist).transpose()[2].astype(float) * 100
angles_diff = np.empty(len(img_dist))
for pp in range(len(img_dist)):
v1 = img_dist[pp][3:6]
v2 = digitized_dist[pp][3:6]
dot_prod = np.dot(v1, v2)
angles_diff[pp] = np.degrees(np.arccos(dot_prod/(np.linalg.norm(v1)*np.linalg.norm(v2))))
f.write("\n")
f.write("Differences between digitized and " + modality + " registration\n")
f.write("\n")
f.write("Marker 1\tMarker 2\tDistance Diff\t\t\tAngle(deg)\n")
for i in range(len(diff)):
f.write(img_dist[i][0][-9:-7] + "\t\t" + img_dist[i][1][
-9:-7] + "\t\t%0.2f mm [%0.2f %%]\t\t%0.2f\n" % (
float(diff[i]), float(diff_percent[i]), angles_diff[i]))
f.write("\n")
f.write("Average \t\t\t%0.2f mm [%0.2f %%]\t\t%0.2f\n" % (np.average(np.abs(diff)), np.average(np.abs(diff_percent)), np.average(np.abs(angles_diff))))
f.write("Maximum \t\t\t%0.2f mm [%0.2f %%]\t\t%0.2f\n" % (np.max(np.abs(diff)), np.max(np.abs(diff_percent)), np.max(np.abs(angles_diff))))
f.close()
def main(dir, segment, modality, reg_id):
seg = ''.join(c for c in segment if c.isupper())
dir1 = os.path.join(dir, "Registration", reg_id)
if not os.path.isdir(dir1):
os.makedirs(dir1)
# Assign seg and bone parameters for later calculations (if certain markers are not used, replace with N/A)
# The markers list represents the digitized order of each set of bone spheres.
if segment == 'UpperLeg':
seg = 'UL_'
bone = 'Femur'
markers = ["F1", "F2", "F3", "F4", "F5", "F6"]
elif segment == 'LowerLeg':
seg = 'LL_'
bone = 'Tibia'
markers = ["T1", "T2", "T3", "T4", "T5", "T6"]
elif segment == 'UpperArm':
seg = 'UA_'
bone = 'Humerus'
markers = ["H1", "H2", "H3", "H4", "N/A", "N/A"]
elif segment == 'LowerArm':
seg = 'LA_'
bone = 'Radius'
markers = ["R1", "N/A", "N/A", "R2", "R3", "N/A"]
# Create XML for results
xml, root = ProbeCoordinates_2.create_xml(dir, '_' + seg[0:-1], segment, modality, reg_id)
# Get accepted trials from Ultrasound experiment
masterList, num_accept = XMLparser.getAcceptance(dir)
accepted_list = []
for x in masterList:
if x[1] == 1:
accepted_list.append(x[0])
dir_data = os.path.join(dir, 'Data')
files = os.listdir(dir_data)
files.sort()
dir_US = os.path.join(dir, 'Ultrasound')
USfiles = os.listdir(dir_US)
USfiles.sort()
for file in files:
if any(file[0:3] in s for s in accepted_list) and seg in file:
data = tdsmParserMultis.parseTDMSfile(os.path.join(dir_data, file))
US_file = next(i for i in USfiles if i[0:3]==file[0:3])
RefDs = dicom.read_file(os.path.join(dir_US, US_file), stop_before_pixels=True)
frameTimeVector = RefDs.FrameTimeVector
US_frame, data_frame, tdms_Frames = getFrames(dir_data, file, frameTimeVector)
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."
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 segmented sphere centers (image coordinate system)
img_dist, spheres, STL_sphere_fit = get_stl_dist(dir, modality, reg_id, markers)
STL_sphere_fit_rm = np.array(map(list, zip(*STL_sphere_fit))[2])
# Singular value decomposition (SVD) method for registration
x = np.array(sphere_fit_rm)#[0:-1] # Use to select only certain STLs if you need to exclude from registration
y = np.array(STL_sphere_fit_rm)/1000
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)
R = np.dot(np.dot(P, np.diag([1, 1, np.linalg.det(np.dot(P, QT))])), QT)
d = y_bar.transpose() - np.dot(R, x_bar.transpose())
# Define transformation matrix from x defined in y (T_I_BOS)
T_I_BOS = np.zeros((4, 4))
T_I_BOS[0:3, 0:3] = R
T_I_BOS[3, 3] = 1
T_I_BOS[0:3, 3] = d
sphere_fit = []
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(T_I_BOS, sphere)
sphere_fit.append([markers[i], s[0], sphere[0:3]*1000])
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(T_I_BOS, sphere)
sphere_fit.append([markers[i+3], s[0], sphere[0:3]*1000])
# sphere_fit = sphere_fit[0:-1] # Use to select only certain STLs if you need to exclude from registration
center_dist = []
for p1 in range(len(sphere_fit) - 1):
for p2 in range(p1 + 1, len(sphere_fit)):
if np.isnan(sphere_fit[p1][2]).all() or np.isnan(sphere_fit[p2][2]).all():
pass
else:
d, x_a, y_a, z_a = get_line(sphere_fit[p1][2].astype(float), sphere_fit[p2][2].astype(float))
center_dist.append([sphere_fit[p1][0], sphere_fit[p2][0], d, x_a, y_a, z_a])
digitized_dist = center_dist
sphere_fit_rm2 = np.array(map(list, zip(*(sphere_fit)))[2])
dist = np.linalg.norm(sphere_fit_rm2-STL_sphere_fit_rm, axis=1)
# Create registration marker quality check text file
if not os.path.exists(os.path.join(dir1, 'QualityCheck')):
os.makedirs(os.path.join(dir1, 'QualityCheck'))
f = open(os.path.join(dir1, 'QualityCheck', reg_id + '_' + os.path.split(dir)[1] + '_MQC_DG_' + modality[0:2] + '_' + seg[0:2] + '.txt'), 'w')
write_quality_check_doc(f, dir, segment, modality, sphere_fit, digitized_dist, STL_sphere_fit, img_dist)
break
print file
for i, tdms_frame in enumerate(tdms_Frames):
probe = config.get('6-DOF Load', 'Description') # Extract the probe that was used during experimentation
# Get the raw sensor data for bone position (TDMS)
B_pos = data[u'Sensor.' + bone]
p_int = data[u'Time'][u'Time'][0]
frame = np.where(np.array(data[u'Time'][u'Time']) == min(list(data[u'Time'][u'Time']),
key=lambda x: abs((x - p_int) - tdms_frame)))[0]
q1 = B_pos[u'' + bone + '_smart_02.x'][frame] / 1000
q2 = B_pos[u'' + bone + '_smart_02.y'][frame] / 1000
q3 = B_pos[u'' + bone + '_smart_02.z'][frame] / 1000
q4 = np.radians(B_pos[u'' + bone + '_smart_02.r'])[frame]
q5 = np.radians(B_pos[u'' + bone + '_smart_02.p'])[frame]
q6 = np.radians(B_pos[u'' + bone + '_smart_02.w'])[frame]
# Calculate the transformation matrix global to bone Optotrak sensor coordinate system
T_W_BOS = get_transformation_matrix(q1, q2, q3, q4, q5, q6)
# Get State File Transformation Matrices
# Transformation matrix from bone Optotrak sensor coordinate system to bone coordinate system
T_BOS_B = config.get('Probe-'+bone+' Position', 't_sensor1_rb1')
T_BOS_B = convertTOarray(T_BOS_B).reshape(4,-1)
# US Optotrak sensor to US tip coordinate system
T_USOS_US = config.get('Probe-'+bone+' Position', 't_sensor2_rb2')
T_USOS_US = convertTOarray(T_USOS_US).reshape(4,-1)
# Get raw ultrasound position (TDMS)
x = np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.x'])[frame]/1000
y = np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.y'])[frame]/1000
z = np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.z'])[frame]/1000
r = np.radians(np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.r'])[frame])
p = np.radians(np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.p'])[frame])
w = np.radians(np.array(data[u'Sensor.US Probe'][u'US Probe_smart_02.w'])[frame])
# Define more transformation matrices (e.g. T_W_USOS is the ultrasound optotrak sensor in the global coordinate system)
T_W_USOS = get_transformation_matrix(x,y,z,r,p,w)
T_W_US = np.dot(T_W_USOS, T_USOS_US)
T_W_B = np.dot(T_W_BOS, T_BOS_B)
T_BOS_US = np.dot(np.linalg.inv(T_W_BOS), T_W_US)
T_I_US = np.dot(T_I_BOS, T_BOS_US)
# Ultrasound probe tip coordinates in the image coordinate system
A = T_I_US # Define what coordinate system to plot everything in
x, y, z, r, p, w = get_xyzrpw(A) # These are the positions that you are looking for (m for xyz, and rad for rpw)
try:
ProbeCoordinates_2.edit_values(segment, file[-11:-9], file[-8:-7], root,
np.array_str(np.array([x, y, z, r, p, w])), file, frame, US_frame[i])
except:
print (segment, file[-11:-9], file[-8:-7], root, np.array_str(np.array([x, y, z, r, p, w])), file, frame)
ProbeCoordinates_2.writeXML(xml, root, dir1)
if __name__ == "__main__":
dir = '/home/morrile2/Documents/MULTIS Data/MULTIS_invitro/CMULTIS012-1' # Cadaver specimen folder
segments = ["UpperLeg", 'LowerLeg', 'UpperArm', 'LowerArm'] # Change this to the desired segment
modalities = ['MRI', 'CT']
registration_ID = 'R01'
for segment in segments:
for modality in modalities:
print segment, modality
main(dir, segment, modality, registration_ID)