def test_readings(self):
cal_readings = "../data/pa2-unknown-h-calreadings.txt"
[D, A, C, frames] = read_cal_readings(cal_readings)
self.assertEqual(frames, 125)
d1 = np.array([-12.30, -5.28, -1499.94])
self.assertEqual(D[0].all(), d1.all())
a1 = np.array([89.53, 96.38, -1400.45])
self.assertEqual(A[0].all(), a1.all())
c1 = np.array([97.77, 100.38, 226.55])
self.assertEqual(C[0].all(), c1.all())
def test_readings(self):
cal_readings = "../data/pa1-unknown-h-calreadings.txt"
[D, A, C, frames] = read_cal_readings(cal_readings)
self.assertEqual(frames, 8)
d1 = np.array([12.17, 3.54, -1499.95])
self.assertEqual(D[0].all(), d1.all())
a1 = np.array([223.42, 210.40, -1286.82])
self.assertEqual(A[0].all(), a1.all())
c1 = np.array([199.23, 238.58, 219.64])
self.assertEqual(C[0].all(), c1.all())
def test_correct_distortion(self):
file_letter = "a"
file_starter = "../data/pa2-debug-"
file_name = file_starter + file_letter + "-output1.txt"
cal_readings = file_starter + file_letter + "-calreadings.txt"
cal_body = file_starter + file_letter + "-calbody.txt"
em_pivot = file_starter + file_letter + "-empivot.txt"
opt_pivot = file_starter + file_letter + "-optpivot.txt"
ct_fiducials = file_starter + file_letter + "-ct-fiducials.txt"
fiducials = file_starter + file_letter + "-em-fiducialss.txt"
em_nav = file_starter + file_letter + "-EM-nav.txt"
# Read all input files
[D, A, C, readings_frames] = read_cal_readings(cal_readings)
[d, a, c] = read_cal_body(cal_body)
[G, G_frames] = read_em_pivot(em_pivot)
[D_pivot, H, H_frames] = read_opt_pivot(opt_pivot)
[Fid, Fid_frames] = read_em_pivot(fiducials)
CTFid = read_ct_fiducials(ct_fiducials)
[EMNav, EMNav_frames] = read_em_pivot(em_nav)
# returns the [x,y,z] of em pivot as a vector
em_pivot_calibration = calibrate_pivot(G, G_frames)
# returns the [x,y,z] of opt pivot as a vector
opt_pivot_calibration = opt2em_calibrate_pivot(H, D_pivot, d, H_frames)
# Part 1: Generate expected C
dstart = 0
astart = 0;
c_expected_list = []
for i in range(0, readings_frames):
[RD, TD] = get_transform(D[dstart:dstart+len(d)], d)
[RA, TA] = get_transform(A[dstart:dstart+len(d)], a)
ca_expected = transform(RA, c.T, TA)
#print 'ca_expected: ' + str(ca_expected)
RDi = np.transpose(RD)
tDi = -1*np.dot(RDi, TD)
c_expected = transform(RDi, ca_expected, tDi)
#print "c expected: " + str(c_expected)
c_expected_list.append(c_expected)
dstart = dstart + len(d)
astart = astart + len(a)
C_calc = []
for i in range(readings_frames):
for j in range(len(c_expected_list[0][0])):
point = [None] * 3
point[0] = c_expected_list[i][0][j]
point[1] = c_expected_list[i][1][j]
point[2] = c_expected_list[i][2][j]
C_calc.append(point)
# Part 2:
# Calculate distortion correction function
[polynomial_coeff, boundbox] = calibrate_distortion(C, C_calc, 5)
# print "polynomial_coeff: " + str(polynomial_coeff)
# Part 3:
# Repeat pivot calibration with distortion correction
G_corrected = np.array(G)
n_g = len(G)/G_frames
# Correct for distortion frame by frame
G_corrected = correct_distortion(G, polynomial_coeff, boundbox)
# print "G: " + str(G)
# print "G_corrected: " + str(G_corrected)
# Return x,y,z of EM pivot in a vector
em_pivot_calibration, p_tip = calibrate_pivot(G_corrected, G_frames)
''' CORRECT DISTORTION TEST '''
g_num = 0
# iterate through the points in G
for g_c in G_corrected:
g_test = G[g_num]
point_num = 0
for g_point in g_c:
# check that corrected in certain error threshold of ground truth G
g_diff = abs(g_point - g_test[point_num])
#print "g_diff: " + str(g_diff)
diff_ratio = g_diff / g_test[point_num]
#print "diff_ratio: " + str(diff_ratio)
point_num += 1
self.assertTrue(diff_ratio < 0.25)
g_num += 1
# Part 4:
# Calculate positions of the fiducial pins
n_fid = len(Fid)/Fid_frames
fiducial_locations = []
for i in range(Fid_frames):
G_corr_frame = G_corrected[i*n_fid:(i+1)*n_fid]
FidData = Fid[i*n_fid:(i+1)*n_fid]
FrameTrans = get_transform(G_corr_frame, FidData)
fiducial_locations.append(transform_3D(FrameTrans, em_pivot_calibration))
# Part 5:
# Calculate the registration transformation between CT image and EM tracker
CTFid = np.array(CTFid)
F_reg = get_transform(fiducial_locations, CTFid)
# Part 6:
# Compute the EM tracked tool's coordinates in the CT image
v = []
EMNav_corrected = np.array(EMNav)
n_nav = len(EMNav)/EMNav_frames
EMNav_corrected = correct_distortion(EMNav, polynomial_coeff, boundbox)
for i in range(0, EMNav_frames):
# Correct for distortion frame by frame
G_corr_frame = G_corrected[i*n_nav:(i+1)*n_nav]
EMNAV_frame = EMNav_corrected[i*n_nav:(i+1)*n_nav, :]
v.append(get_tip_coordinates(em_pivot_calibration, EMNAV_frame, G_corr_frame,F_reg))
p2_output = "../data/pa2-debug-" + file_letter + "-output2.txt"
p2_output_name = "pa2-debug-" + file_letter + "-output2.txt"
output = open(p2_output, 'r')
output_val = []
first_line = output.readline()
for i in range(4):
current_line = output.readline().split(",")
for x in current_line:
x.strip()
x_pos = float(current_line[0])
y_pos = float(current_line[1])
z_pos = float(current_line[2])
point = [x_pos, y_pos, z_pos]
output_val.append(point)
''' TEST THAT FINAL OUTPUT WITHIN THRESHOLD '''
for i in range(4):
for j in range(3):
output_point = output_val[i][j]
v_point = v[i][j]
point_diff = abs(output_point - v_point)
diff_ratio = point_diff / output_point
# print "diff ratio: " + str(diff_ratio)
self.assertTrue(diff_ratio < 0.15)
def main():
# the total number of datasets to be run on
num_datasets = 11
# the initial dataset letter
file_letter = 'a'
for dataset_num in range(num_datasets):
# strings for building the requested file address
file_starter = "../data/pa1-debug-"
file_name = file_starter + file_letter + "-output1.txt"
open_test = os.path.isfile(file_name)
# if file is not debug, change starter to unknown
if not open_test:
file_starter = "../data/pa1-unknown-"
# file endings for each dataset
cal_readings = file_starter + file_letter + "-calreadings.txt"
cal_body = file_starter + file_letter + "-calbody.txt"
em_pivot = file_starter + file_letter + "-empivot.txt"
opt_pivot = file_starter + file_letter + "-optpivot.txt"
# run parser functions for each dataset, saving return values
# as numpy arrays
[D, A, C, frames] = read_cal_readings(cal_readings)
[d, a, c] = read_cal_body(cal_body)
[G, G_frames] = read_em_pivot(em_pivot)
[D_pivot, H, H_frames] = read_opt_pivot(opt_pivot)
# returns the [x,y,z] of em pivot as a vector
em_pivot_calibration = calibrate_pivot(G, G_frames)
# returns the [x,y,z] of opt pivot as a vector
opt_pivot_calibration = opt2em_calibrate_pivot(H, D_pivot, d, H_frames)
# the current frame of d and a to be transformed
dstart = 0
astart = 0
c_expected_list = []
for i in range(0, frames):
# get the rotation and translation matrices between the
# reading and body values for D and A
[RD, TD] = get_transform(D[dstart:dstart + len(d)], d)
[RA, TA] = get_transform(A[dstart:dstart + len(d)], a)
# first apply the A transformation to body frame c
ca_expected = transform(RA, c.T, TA)
RDi = np.transpose(RD)
tDi = -1 * np.dot(RDi, TD)
# then apply the inverse D transform
c_expected = transform(RDi, ca_expected, tDi)
# append the result
c_expected_list.append(c_expected)
# iterate to the next frame
dstart = dstart + len(d)
astart = astart + len(a)
# strings for building the output file
output_file = "../output/pa1-" + file_letter + "-output1.txt"
output_file_name = "pa1-" + file_letter + "-output1.txt"
output = open(output_file, 'w+')
# writes the expected em and opt pivot positions
output.write("EM pivot post est position: ")
output.write(
str(em_pivot_calibration[0][0]) + ",\t" +
str(em_pivot_calibration[0][1]) + ",\t" +
str(em_pivot_calibration[0][2]) + "\n")
output.write("Optical pivot post est position: ")
output.write(
str(opt_pivot_calibration[0][0]) + ",\t" +
str(opt_pivot_calibration[0][1]) + ",\t" +
str(opt_pivot_calibration[0][2]) + "\n")
# iterates through expected c frames and prints them to output file
for c in c_expected_list:
numrow, numcol = c.shape
for row in range(numcol):
outstring = ''
for col in range(numrow):
outstring += str(c[col, row]) + ', '
output.write(outstring)
output.write('\n')
output.close()
# Print the difference between each C and C_expected
output_diff_file = "../output/pa1-" + file_letter + "-output-difference.txt"
open_test = os.path.isfile(file_name)
output_diff = open(output_diff_file, 'w+')
# iterate through c_expected
c_num = 0
total_diff = 0
for c in c_expected_list:
for row in range(numcol):
outstring = ''
for col in range(numrow):
# find difference in C
c_diff = c[col, row] - C[c_num, col]
# add to total to find average difference
total_diff += abs(c_diff)
# print "Next C: " + str(C[c_num, col])
outstring += str(c_diff) + ', '
output_diff.write(outstring)
output_diff.write('\n')
c_num += 1
average_diff = total_diff / (c_num * 3)
# Print the average C error
output_diff.write("Average difference: " + str(average_diff))
# Increment to next letter dataset
file_letter = chr(ord(file_letter) + 1)
output_diff.close()
print(
"Program ran successfully! Check output folder for output files for each dataset."
)