def test_transform_2(self):
a = np.array([[5,7,1], [6,3,2], [6,9,1], [1,3,7], [8,5,3]])
rotation = np.array([[1,0,0], [0,1,0], [0,0,1]])
translation = np.array([6,3,5])
identity = np.array([[1,0,0], [0,1,0], [0,0,1]])
ra = np.dot(rotation, a.T)
b = np.add(ra, np.vstack(translation))
[R1, t1] = get_transform(a, b.T)
[R2 ,t2] = get_transform(b.T, a)
result = np.add(np.add(np.dot(R1, R2), t1), t2)
self.assertEqual(result.all(), identity.all())
def calibrate_pivot(G, num_frames):
# Inputs:
# G -> Set of multiple frames of data where the tool is in contact with a fiducial pin
# num_frames -> The number of data frames in G
# Outputs:
# p_tip -> The vector from the tool frame to the end of the tool in contact with the post
# p_post -> The vector from the EM base frame to the top of the fiducial post
# Given a set of EM tracker readings
# Produce a transform between F_G to F_g on the probe
# Use this frame transformation to produce a set of points g
# Which are G localized to the probe
G = np.array(G)
F_Gg = []
n = len(G) / num_frames
G_0 = compute_midpoint(G[0:n, :])
g_k = localize(G_0, G[0:n, :])
for i in range(0, num_frames):
F_Gg.append(get_transform(g_k, G[i * (n):(i + 1) * (n), :]))
# Then solve for b_post and b_tip?
p_post, p_tip = pivot_calibration(F_Gg, num_frames)
return p_post, p_tip
def opt2em_calibrate_pivot(H, D, d, num_frames):
# Inputs:
# H -> Set of multiple frames of data where the tool is in contact with a fiducial pin in the Optical Tracker Frame
# D -> Set of multiple frames of data of the distance between the Optical and EM tracker
# d -> Set of optical trackers on the EM frame
# num_frames -> The number of data frames in H
# Outputs:
# p_tip -> The vector from the tool frame to the end of the tool in contact with the post
# p_post -> The vector from the EM base frame to the top of the fiducial post
# Calculate a transform between the EM and the Optical tracker
# for each frame of data
H = np.array(H)
D = np.array(D)
d = np.array(d)
F_d = []
n_H = len(H) / num_frames
n_D = len(D) / num_frames
for i in range(0, num_frames):
F_d.append(get_transform(d, D[i * (n_D):(i + 1) * (n_D), :]))
# Transform each H into the Em tracker frame using these F_d
# for each frame of data
H2 = []
for i in range(0, num_frames):
for j in range(0, n_H):
H2.append(transform_3D(F_d[i], H[i * n_H + j]))
H2 = np.array(H2)
# Return the coordinates of the post the optically tracked
# tool is being calibrated on
return calibrate_pivot(H2, num_frames)
def opt2em_calibrate_pivot(H, D, d, num_frames):
# Calculate a transform between the EM and the Optical tracker
# for each frame of data
H = np.array(H)
D = np.array(D)
d = np.array(d)
F_d = []
n_H = len(H) / num_frames
n_D = len(D) / num_frames
for i in range(0, num_frames):
F_d.append(get_transform(d, D[i * (n_D):(i + 1) * (n_D), :]))
# Transform each H into the Em tracker frame using these F_d
# for each frame of data
H2 = []
for i in range(0, num_frames):
for j in range(0, n_H):
H2.append(transform_3D(F_d[i], H[i * n_H + j]))
H2 = np.array(H2)
# Return the coordinates of the post the optically tracked
# tool is being calibrated on
return calibrate_pivot(H2, num_frames)
def get_tip_coordinates(p_post, G_new, G_old, F_reg):
F_G = get_transform(G_old, G_new)
v1 = transform_3D(F_G, p_post)
v2 = transform_3D(F_reg, v1)
return v2
def calibrate_pivot_one_frame(G, p_tip):
G_0 = compute_midpoint(G)
g_k = localize(G_0, G)
F = get_transform(G, g_k)
p_post = np.dot(F[0], p_tip) - F[1]
return p_post
def test_transform(self):
a = np.array([[5,7,1], [6,3,2], [6,9,1], [1,3,7], [8,5,3]])
rotation = np.array([[1,0,0], [0,1,0], [0,0,1]])
translation = np.array([6,3,5])
ra = np.dot(rotation, a.T)
b = np.add(ra, np.vstack(translation))
[R, t] = get_transform(a, b.T)
self.assertEqual(R.all(), rotation.all())
self.assertEqual(t.all(), translation.all())
def calibrate_pivot(G, num_frames):
G = np.array(G)
F_Gg = []
n = len(G) / num_frames
G_0 = compute_midpoint(G[0:n, :])
g_k = localize(G_0, G[0:n, :])
for i in range(0, num_frames):
F_Gg.append(get_transform(g_k, G[i * (n):(i + 1) * (n), :]))
# Then solve for b_post and b_tip?
p_post, p_tip = pivot_calibration(F_Gg, num_frames)
return p_post, p_tip
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."
)
