|
|
|
@@ -16,6 +16,82 @@ xy_subplot = plt.subplot2grid((1, 2), (0, 1)) |
|
|
|
yz_subplot = plt.subplot2grid((1, 2), (0, 0)) |
|
|
|
|
|
|
|
|
|
|
|
# Load all the IndLoc Recordings |
|
|
|
center = np.load("recorded_data/center.npy")[1:, :, :] |
|
|
|
off_center = np.load("recorded_data/off_center.npy")[1:, :, :] |
|
|
|
horizontal = np.load("recorded_data/horizontal.npy")[1:, :, :] |
|
|
|
diagonal = np.load("recorded_data/diagonal.npy")[1:, :, :] |
|
|
|
zigzag_1 = np.load("recorded_data/zigzag_1.npy") |
|
|
|
zigzag_2 = np.load("recorded_data/zigzag_2.npy") |
|
|
|
wave_1 = np.load("recorded_data/wave_1.npy") |
|
|
|
wave_2 = np.load("recorded_data/wave_2.npy") |
|
|
|
spiral_1 = np.load("recorded_data/spiral_1.npy") |
|
|
|
half_current_horizontal = np.load("recorded_data/half_current_horizontal.npy")[1:, :, :] |
|
|
|
half_current_diagonal = np.load("recorded_data/half_current_diagonal.npy")[1:, :, :] |
|
|
|
double_current_horizontal = np.load("recorded_data/double_current_horizontal.npy")[1:, :, :] |
|
|
|
double_current_diagonal = np.load("recorded_data/half_current_diagonal.npy")[1:, :, :] |
|
|
|
z_axis = np.load("recorded_data/z_axis.npy")[1:, :, :] |
|
|
|
z_axis_off_center = np.load("recorded_data/z_axis_off_center.npy")[1:, :, :] |
|
|
|
Three_D_movement_1 = np.load("recorded_data/3D_movement_1.npy") |
|
|
|
Three_D_movement_2 = np.load("recorded_data/3D_movement_2.npy") |
|
|
|
Three_D_zigzag_1 = np.load("recorded_data/3D_zigzag_1.npy") |
|
|
|
Three_D_zigzag_2 = np.load("recorded_data/3D_zigzag_2.npy") |
|
|
|
z_axis_moving_1 = np.load("recorded_data/z_axis_moving_1.npy") |
|
|
|
# z_axis_moving_2 = np.load("recorded_data/z_axis_moving_2.npy") # Recording missing? |
|
|
|
z_wave_1 = np.load("recorded_data/z_wave_1.npy") |
|
|
|
z_wave_2 = np.load("recorded_data/z_wave_2.npy") |
|
|
|
|
|
|
|
recordings_dict = {"center": center, # shape: (26, 2000, 19) = [P1-P25, 2k samples, fv] |
|
|
|
"off_center": off_center, |
|
|
|
"horizontal": horizontal, |
|
|
|
"diagonal": diagonal, |
|
|
|
"horizontal_and_diagonal": np.append(horizontal, diagonal, axis=0), |
|
|
|
"zigzag_1": zigzag_1, |
|
|
|
"zigzag_2": zigzag_2, |
|
|
|
"wave_1": wave_1, |
|
|
|
"wave_2": wave_2, |
|
|
|
"spiral_1": spiral_1, |
|
|
|
"half_current_horizontal": half_current_horizontal, |
|
|
|
"half_current_diagonal": half_current_diagonal, |
|
|
|
"double_current_horizontal": double_current_horizontal, |
|
|
|
"double_current_diagonal": double_current_diagonal, |
|
|
|
"z_axis": z_axis, |
|
|
|
"z_axis_off_center": z_axis_off_center, |
|
|
|
"Three_D_movement_1": Three_D_movement_1, |
|
|
|
"Three_D_movement_2": Three_D_movement_2, |
|
|
|
"Three_D_zigzag_1": Three_D_zigzag_1, |
|
|
|
"Three_D_zigzag_2": Three_D_zigzag_2, |
|
|
|
"z_axis_moving_1": z_axis_moving_1, |
|
|
|
"z_wave_1": z_wave_1, |
|
|
|
"z_wave_2": z_wave_2} |
|
|
|
|
|
|
|
|
|
|
|
# this only takes the first sample of each recording |
|
|
|
recording_dict_fs = {"center": center[:, 0, :], |
|
|
|
"off_center": off_center[:, 0, :], |
|
|
|
"horizontal": horizontal[:, 0, :], |
|
|
|
"diagonal": diagonal[:, 0, :], |
|
|
|
"horizontal_and_diagonal": recordings_dict["horizontal_and_diagonal"][:, 0, :], |
|
|
|
"zigzag_1": zigzag_1[0, :], |
|
|
|
"zigzag_2": zigzag_2[0, :], |
|
|
|
"wave_1": wave_1[0, :], |
|
|
|
"wave_2": wave_2[0, :], |
|
|
|
"spiral_1": spiral_1[0, :], |
|
|
|
"half_current_horizontal": half_current_horizontal[:, 0, :], |
|
|
|
"half_current_diagonal": half_current_diagonal[:, 0, :], |
|
|
|
"double_current_horizontal": double_current_horizontal[:, 0, :], |
|
|
|
"double_current_diagonal": double_current_diagonal[:, 0, :], |
|
|
|
"z_axis": z_axis[:, 0, :], |
|
|
|
"z_axis_off_center": z_axis_off_center[:, 0, :], |
|
|
|
"Three_D_movement_1": Three_D_movement_1[0, :], |
|
|
|
"Three_D_movement_2": Three_D_movement_2[0, :], |
|
|
|
"Three_D_zigzag_1": Three_D_zigzag_1[0, :], |
|
|
|
"Three_D_zigzag_2": Three_D_zigzag_2[0, :], |
|
|
|
"z_axis_moving_1": z_axis_moving_1[0, :], |
|
|
|
"z_wave_1": z_wave_1[0, :], |
|
|
|
"z_wave_2": z_wave_2[0, :]} |
|
|
|
|
|
|
|
|
|
|
|
def plot_surroundings(): |
|
|
|
font = {"family": "serif", "color": "black", "size": 15} |
|
|
|
|
|
|
|
@@ -130,11 +206,7 @@ def plot_surroundings(): |
|
|
|
# yz_subplot.add_patch(rect2) |
|
|
|
|
|
|
|
|
|
|
|
plot_art_call_counter = 1 # used for annotating P1 to P25 |
|
|
|
|
|
|
|
|
|
|
|
def plot_art(recording_name, annotate_point): |
|
|
|
global plot_art_call_counter |
|
|
|
def art_file_to_position(recording_name): |
|
|
|
""" |
|
|
|
:param recording_name: int, Choose which recording to print e.g. Recording_!1! , Recording_!2!, etc. |
|
|
|
""" |
|
|
|
@@ -173,105 +245,30 @@ def plot_art(recording_name, annotate_point): |
|
|
|
|
|
|
|
pos_counter += 1 |
|
|
|
|
|
|
|
if label_flag: # only add the label to one of the points |
|
|
|
xy_subplot.scatter(art_info[0, 1], art_info[0, 2], color="green", label="ART System", s=100, marker="x") |
|
|
|
yz_subplot.scatter(art_info[0, 3], art_info[0, 2], color="green", label="ART System", s=100, marker="x") |
|
|
|
else: |
|
|
|
xy_subplot.scatter(art_info[0, 1], art_info[0, 2], color="green", s=100, marker="x") |
|
|
|
yz_subplot.scatter(art_info[0, 3], art_info[0, 2], color="green", s=100, marker="x") |
|
|
|
|
|
|
|
#print("ART: x=", art_info[0, 1], ", y=", art_info[0, 2], ", z=", art_info[0, 3]) |
|
|
|
|
|
|
|
if annotate_point: |
|
|
|
xy_subplot.annotate("P" + str(plot_art_call_counter), (art_info[0, 1], art_info[0, 2]), color="green", size=15) |
|
|
|
yz_subplot.annotate("P" + str(plot_art_call_counter), (art_info[0, 3], art_info[0, 2]), color="green", size=15) |
|
|
|
plot_art_call_counter += 1 |
|
|
|
|
|
|
|
# print("x=", art_info[:, 1]) |
|
|
|
# print("y=", art_info[:, 2]) |
|
|
|
pos = [art_info[0, 1], art_info[0, 2], art_info[0, 3]] |
|
|
|
pos = [art_info[0, 1], art_info[0, 2], art_info[0, 3]] # only take the first x,y,z position of each recording (as the 0.33mm precision wont have a lot of noise anyways) |
|
|
|
return pos |
|
|
|
|
|
|
|
center = np.load("recorded_data/center.npy")[1:, :, :] |
|
|
|
off_center = np.load("recorded_data/off_center.npy")[1:, :, :] |
|
|
|
horizontal = np.load("recorded_data/horizontal.npy")[1:, :, :] |
|
|
|
diagonal = np.load("recorded_data/diagonal.npy")[1:, :, :] |
|
|
|
zigzag_1 = np.load("recorded_data/zigzag_1.npy") |
|
|
|
zigzag_2 = np.load("recorded_data/zigzag_2.npy") |
|
|
|
wave_1 = np.load("recorded_data/wave_1.npy") |
|
|
|
wave_2 = np.load("recorded_data/wave_2.npy") |
|
|
|
spiral_1 = np.load("recorded_data/spiral_1.npy") |
|
|
|
half_current_horizontal = np.load("recorded_data/half_current_horizontal.npy")[1:, :, :] |
|
|
|
half_current_diagonal = np.load("recorded_data/half_current_diagonal.npy")[1:, :, :] |
|
|
|
double_current_horizontal = np.load("recorded_data/double_current_horizontal.npy")[1:, :, :] |
|
|
|
double_current_diagonal = np.load("recorded_data/half_current_diagonal.npy")[1:, :, :] |
|
|
|
z_axis = np.load("recorded_data/z_axis.npy")[1:, :, :] |
|
|
|
z_axis_off_center = np.load("recorded_data/z_axis_off_center.npy")[1:, :, :] |
|
|
|
Three_D_movement_1 = np.load("recorded_data/3D_movement_1.npy") |
|
|
|
Three_D_movement_2 = np.load("recorded_data/3D_movement_2.npy") |
|
|
|
Three_D_zigzag_1 = np.load("recorded_data/3D_zigzag_1.npy") |
|
|
|
Three_D_zigzag_2 = np.load("recorded_data/3D_zigzag_2.npy") |
|
|
|
z_axis_moving_1 = np.load("recorded_data/z_axis_moving_1.npy") |
|
|
|
# z_axis_moving_2 = np.load("recorded_data/z_axis_moving_2.npy") # Recording missing? |
|
|
|
z_wave_1 = np.load("recorded_data/z_wave_1.npy") |
|
|
|
z_wave_2 = np.load("recorded_data/z_wave_2.npy") |
|
|
|
|
|
|
|
recordings_dict = {"center": center, # shape: (26, 2000, 19) = [P1-P25, 2k samples, fv] |
|
|
|
"off_center": off_center, |
|
|
|
"horizontal": horizontal, |
|
|
|
"diagonal": diagonal, |
|
|
|
"horizontal_and_diagonal": np.append(horizontal, diagonal, axis=0), |
|
|
|
"zigzag_1": zigzag_1, |
|
|
|
"zigzag_2": zigzag_2, |
|
|
|
"wave_1": wave_1, |
|
|
|
"wave_2": wave_2, |
|
|
|
"spiral_1": spiral_1, |
|
|
|
"half_current_horizontal": half_current_horizontal, |
|
|
|
"half_current_diagonal": half_current_diagonal, |
|
|
|
"double_current_horizontal": double_current_horizontal, |
|
|
|
"double_current_diagonal": double_current_diagonal, |
|
|
|
"z_axis": z_axis, |
|
|
|
"z_axis_off_center": z_axis_off_center, |
|
|
|
"Three_D_movement_1": Three_D_movement_1, |
|
|
|
"Three_D_movement_2": Three_D_movement_2, |
|
|
|
"Three_D_zigzag_1": Three_D_zigzag_1, |
|
|
|
"Three_D_zigzag_2": Three_D_zigzag_2, |
|
|
|
"z_axis_moving_1": z_axis_moving_1, |
|
|
|
"z_wave_1": z_wave_1, |
|
|
|
"z_wave_2": z_wave_2} |
|
|
|
|
|
|
|
# print("recordings_dict: ", recordings_dict["zigzag_1"]) |
|
|
|
# print("shape: recordings_dict: ", np.shape(recordings_dict["zigzag_1"])) |
|
|
|
def plot_art_positions(positions, annotate_flag, markersize, annotation_size): |
|
|
|
|
|
|
|
# this only takes the first sample of each recording |
|
|
|
recording_dict_fs = {"center": center[:, 0, :], |
|
|
|
"off_center": off_center[:, 0, :], |
|
|
|
"horizontal": horizontal[:, 0, :], |
|
|
|
"diagonal": diagonal[:, 0, :], |
|
|
|
"horizontal_and_diagonal": recordings_dict["horizontal_and_diagonal"][:, 0, :], |
|
|
|
"zigzag_1": zigzag_1[0, :], |
|
|
|
"zigzag_2": zigzag_2[0, :], |
|
|
|
"wave_1": wave_1[0, :], |
|
|
|
"wave_2": wave_2[0, :], |
|
|
|
"spiral_1": spiral_1[0, :], |
|
|
|
"half_current_horizontal": half_current_horizontal[:, 0, :], |
|
|
|
"half_current_diagonal": half_current_diagonal[:, 0, :], |
|
|
|
"double_current_horizontal": double_current_horizontal[:, 0, :], |
|
|
|
"double_current_diagonal": double_current_diagonal[:, 0, :], |
|
|
|
"z_axis": z_axis[:, 0, :], |
|
|
|
"z_axis_off_center": z_axis_off_center[:, 0, :], |
|
|
|
"Three_D_movement_1": Three_D_movement_1[0, :], |
|
|
|
"Three_D_movement_2": Three_D_movement_2[0, :], |
|
|
|
"Three_D_zigzag_1": Three_D_zigzag_1[0, :], |
|
|
|
"Three_D_zigzag_2": Three_D_zigzag_2[0, :], |
|
|
|
"z_axis_moving_1": z_axis_moving_1[0, :], |
|
|
|
"z_wave_1": z_wave_1[0, :], |
|
|
|
"z_wave_2": z_wave_2[0, :]} |
|
|
|
for i in range(len(positions)): |
|
|
|
if i == 0: # only add the label to one of the points |
|
|
|
xy_subplot.scatter(positions[i, 0], positions[i, 1], color="green", label="ART System", s=markersize, marker="x") |
|
|
|
yz_subplot.scatter(positions[i, 2], positions[i, 1], color="green", label="ART System", s=markersize, marker="x") |
|
|
|
else: |
|
|
|
xy_subplot.scatter(positions[i, 0], positions[i, 1], color="green", s=markersize, marker="x") |
|
|
|
yz_subplot.scatter(positions[i, 2], positions[i, 1], color="green", s=markersize, marker="x") |
|
|
|
|
|
|
|
#print("ART: x=", art_info[0, 1], ", y=", art_info[0, 2], ", z=", art_info[0, 3]) |
|
|
|
|
|
|
|
if annotate_flag: |
|
|
|
xy_subplot.annotate("P" + str(i+1), (positions[i, 0], positions[i, 1]), color="green", size=annotation_size) |
|
|
|
yz_subplot.annotate("P" + str(i+1), (positions[i, 2], positions[i, 1]), color="green", size=annotation_size) |
|
|
|
|
|
|
|
|
|
|
|
def calc_multiple_positions(recording_name, only_fs, sf_list, k_list): |
|
|
|
def calc_multiple_positions(data, output_filename, only_fs, sf_list, k_list): |
|
|
|
""" |
|
|
|
Takes a recording name and calculates a lot of positions for it, for each given scaling_factor and k_nearest_factor |
|
|
|
and stores them all as numpy arrays in folder "calculated_positions/..." |
|
|
|
@@ -279,28 +276,45 @@ def calc_multiple_positions(recording_name, only_fs, sf_list, k_list): |
|
|
|
:param only_fs: only take first sample of recording |
|
|
|
:param sf_list: list with all scaling factors to localize with |
|
|
|
:param k_list: list with all k_nearest factors to localize with |
|
|
|
:param output_filename: it saves the positions under this name. It adds stuff like the scaling factor , k-nearest automatically |
|
|
|
:return returns an array containing all positions [3, len(sf_list)*len(k_list)] |
|
|
|
""" |
|
|
|
if only_fs: |
|
|
|
data = recording_dict_fs[recording_name] |
|
|
|
else: |
|
|
|
data = recordings_dict[recording_name] |
|
|
|
|
|
|
|
positions = np.zeros((len(sf_list)*len(k_list), np.shape(data)[0], 3)) # shape: [1000, 42, 3] = [Combinations of sf and k, P1-P42, 3 xyz] |
|
|
|
|
|
|
|
path_list = [] |
|
|
|
i = 0 |
|
|
|
j = 0 |
|
|
|
print("----Calculating positions for ", len(sf_list) * len(k_list), " combinations of sf and k-----") |
|
|
|
for sf in sf_list: |
|
|
|
localization.scale_factor = sf |
|
|
|
for k in k_list: |
|
|
|
localization.k_nearest = k |
|
|
|
localization.localize_all_samples_direct(data, recording_name + "_sf=" + str(sf) + "_k=" + str(k)) |
|
|
|
path_list.append("calculated_positions/"+recording_name + "_sf=" + str(sf) + "_k=" + str(k)+".npy") |
|
|
|
positions[(i+j), :, :] = localization.localize_all_samples_direct(data, output_filename + "_sf=" + str(sf) + "_k=" + str(k)) |
|
|
|
|
|
|
|
if len(k_list) != 1: # only increment this if we have multiple k_nearests getting tested |
|
|
|
j += 1 |
|
|
|
if len(sf_list) != 1: # only increment i if we have mutliple scaling factors getting tested |
|
|
|
i += 1 |
|
|
|
print("progress ", i, "/", (len(sf_list)*len(k_list))) |
|
|
|
print("----Finished all position calculations-----") |
|
|
|
for path in path_list: |
|
|
|
print(path) |
|
|
|
return positions |
|
|
|
|
|
|
|
|
|
|
|
def plot_multiple_positions(recording_name, sf_list, k_list, only_label_once, xy_title, yz_title, annotate_points, labels, color_all_black): |
|
|
|
global plot_art_call_counter |
|
|
|
def plot_multiple_positions(positions_filename, sf_list, k_list, only_label_once, xy_title, yz_title, annotate_points, labels, color_all_black, markersize, annotationsize): |
|
|
|
""" |
|
|
|
You can give this function the name of an IndLoc recording and a bunch of scaling factors and k-nearests |
|
|
|
and it will plot you all of them. |
|
|
|
:param recording_name: |
|
|
|
:param sf_list: |
|
|
|
:param k_list: |
|
|
|
:param only_label_once: |
|
|
|
:param xy_title: |
|
|
|
:param yz_title: |
|
|
|
:param annotate_points: |
|
|
|
:param labels: |
|
|
|
:param color_all_black: |
|
|
|
:return: |
|
|
|
""" |
|
|
|
colors = iter(plt.cm.jet(np.linspace(0, 1, len(sf_list)*len(k_list)))) |
|
|
|
#markers = iter(['o', '8', 'p', '>', 'd', 'H', 'x','v', '^','*', 'D','s', 'h', '+', '<']) |
|
|
|
markers = iter(['x', 'x']) |
|
|
|
@@ -310,23 +324,23 @@ def plot_multiple_positions(recording_name, sf_list, k_list, only_label_once, xy |
|
|
|
for k in k_list: |
|
|
|
color = next(colors) |
|
|
|
marker = next(markers) |
|
|
|
positions = np.load("calculated_positions/"+recording_name + "_sf=" + str(sf) + "_k=" + str(k)+".npy") |
|
|
|
positions = np.load("calculated_positions/"+positions_filename + "_sf=" + str(sf) + "_k=" + str(k)+".npy") |
|
|
|
# print("Plotting positions for: sf=", sf, ", k=", k, positions) # show all positions |
|
|
|
print("Plotting positions for: sf=", sf, ", k=", k) # minimal print view |
|
|
|
if color_all_black: |
|
|
|
xy_subplot.scatter(positions[:, 0], positions[:, 1], marker=marker, color="black", label=next(labels), s=100) |
|
|
|
yz_subplot.scatter(positions[:, 2], positions[:, 1], marker=marker, color="black", s=100) |
|
|
|
xy_subplot.scatter(positions[:, 0], positions[:, 1], marker=marker, color="black", label=next(labels), s=markersize) |
|
|
|
yz_subplot.scatter(positions[:, 2], positions[:, 1], marker=marker, color="black", s=markersize) |
|
|
|
if annotate_points: |
|
|
|
for i in range(len(positions[:, 0])): |
|
|
|
xy_subplot.annotate("P" + str(i + 1), (positions[i, 0], positions[i, 1]), color="black", size=15) |
|
|
|
yz_subplot.annotate("P" + str(i + 1), (positions[i, 2], positions[i, 1]), color="black", size=15) |
|
|
|
xy_subplot.annotate("P" + str(i + 1), (positions[i, 0], positions[i, 1]), color="black", size=annotationsize) |
|
|
|
yz_subplot.annotate("P" + str(i + 1), (positions[i, 2], positions[i, 1]), color="black", size=annotationsize) |
|
|
|
else: |
|
|
|
xy_subplot.scatter(positions[:, 0], positions[:, 1], marker=marker, facecolor=color, label=next(labels)) |
|
|
|
yz_subplot.scatter(positions[:, 2], positions[:, 1], marker=marker, facecolor=color) |
|
|
|
if annotate_points: |
|
|
|
for i in range(len(positions[:, 0])): |
|
|
|
xy_subplot.annotate("P" + str(i + 1), (positions[i, 0], positions[i, 1]), color=color, size=15) |
|
|
|
yz_subplot.annotate("P" + str(i + 1), (positions[i, 2], positions[i, 1]), color=color, size=15) |
|
|
|
xy_subplot.annotate("P" + str(i + 1), (positions[i, 0], positions[i, 1]), color=color, size=annotationsize) |
|
|
|
yz_subplot.annotate("P" + str(i + 1), (positions[i, 2], positions[i, 1]), color=color, size=annotationsize) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@@ -334,124 +348,168 @@ def plot_multiple_positions(recording_name, sf_list, k_list, only_label_once, xy |
|
|
|
xy_subplot.set_title(str(yz_title), size=20) |
|
|
|
|
|
|
|
|
|
|
|
def eval_loc(IndLoc_pos, ART_pos): |
|
|
|
""" |
|
|
|
Takes 2 arrays full of x,y,z positions. Calculated the distance between the two for each position. |
|
|
|
Then calculates the distance for each point. Then the mean_distance and the standard_deviation of all positions together. |
|
|
|
|
|
|
|
recording_selection = "horizontal_and_diagonal" |
|
|
|
:param IndLoc_pos: np.array[i, 3] (a number of positions: x,y,z) |
|
|
|
:param ART_pos: np.array[i, 3] (a number of positions: x,y,z) |
|
|
|
:return: distances, mean_distance, std_dev_distance |
|
|
|
""" |
|
|
|
|
|
|
|
if np.shape(IndLoc_pos) != np.shape(ART_pos): |
|
|
|
print("During calc distance function. Positions arrays dont have the same shape. \n IndLoc (shape):", |
|
|
|
np.shape(IndLoc_pos), " ART (shape):", np.shape(ART_pos)) |
|
|
|
else: |
|
|
|
distances = np.zeros((np.shape(IndLoc_pos)[0], 1)) # create an array containing all distances |
|
|
|
for i in range(np.shape(IndLoc_pos)[0]): # iterate through all positions |
|
|
|
x_IndLoc = IndLoc_pos[i, 0] |
|
|
|
y_IndLoc = IndLoc_pos[i, 1] |
|
|
|
z_IndLoc = IndLoc_pos[i, 2] |
|
|
|
|
|
|
|
x_ART = ART_pos[i, 0] |
|
|
|
y_ART = ART_pos[i, 1] |
|
|
|
z_ART = ART_pos[i, 2] |
|
|
|
|
|
|
|
distance = math.sqrt( |
|
|
|
(x_IndLoc - x_ART) ** 2 + (y_IndLoc - y_ART) ** 2) # calculate difference for each position |
|
|
|
distances[i] = distance |
|
|
|
# print("i", i) |
|
|
|
# print("IndLoc: x=", x_IndLoc, ", y=", y_IndLoc) |
|
|
|
# print("ART: x=", x_ART, ", y=", y_ART) |
|
|
|
|
|
|
|
mean_distance = np.mean(distances) |
|
|
|
std_dev_distance = np.std(distances) |
|
|
|
|
|
|
|
return distances, round(mean_distance, 2), round(std_dev_distance, 2) |
|
|
|
|
|
|
|
|
|
|
|
def load_entire_ART(recording_names): |
|
|
|
""" |
|
|
|
:param recording_names: list of all recording that are to be loaded ["center", "horizontal", "diagonal"] |
|
|
|
:return: an array containing them all appended together |
|
|
|
""" |
|
|
|
x_list = [] |
|
|
|
y_list = [] |
|
|
|
z_list = [] |
|
|
|
|
|
|
|
for i in range(len(recording_names)): # iterate through list of wanted recordings |
|
|
|
print("Loading ART", recording_names[i]) |
|
|
|
for j in range(1, 30): # it always starts with "P1 and ends variable but never over P30" |
|
|
|
try: |
|
|
|
current_pos = art_file_to_position(recording_names[i]+"_P"+(str(j))) |
|
|
|
x_list.append(current_pos[0]) |
|
|
|
y_list.append(current_pos[1]) |
|
|
|
z_list.append(current_pos[2]) |
|
|
|
except FileNotFoundError: |
|
|
|
break |
|
|
|
|
|
|
|
pos = np.zeros((len(x_list), 3)) |
|
|
|
|
|
|
|
pos[:, 0] = x_list |
|
|
|
pos[:, 1] = y_list |
|
|
|
pos[:, 2] = z_list |
|
|
|
|
|
|
|
return pos |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# Normal ART Loading (1 Recording) |
|
|
|
# for i in range(1, 26): |
|
|
|
# print("i", i) |
|
|
|
# # if i <= 21: |
|
|
|
# # pos = plot_art("horizontal_P" + str(i), annotate_point=False) |
|
|
|
# if i == 26: |
|
|
|
# label_flag = True |
|
|
|
# pos = plot_art(recording_selection+"_P" + str(i), annotate_point=False) |
|
|
|
# x_list.append(pos[0]) |
|
|
|
# y_list.append(pos[1]) |
|
|
|
# z_list.append(pos[2]) |
|
|
|
|
|
|
|
|
|
|
|
# Make the plot look nice |
|
|
|
plot_surroundings() |
|
|
|
|
|
|
|
# Load ART positions |
|
|
|
ART_pos = load_entire_ART(["double_current_horizontal", "double_current_diagonal"]) |
|
|
|
|
|
|
|
# Load IndLoc data |
|
|
|
horizontal = recordings_dict["double_current_horizontal"] |
|
|
|
diagonal = recordings_dict["double_current_diagonal"] |
|
|
|
IndLoc_data = np.append(horizontal, diagonal, axis=0) # shape (42, 2000, 19) = (P1-P42, 2k samples, fv) |
|
|
|
|
|
|
|
# Average IndLoc data |
|
|
|
IndLoc_data = np.average(IndLoc_data, axis=1) # shape (42, 19) = (P1-P42, fv_avg) |
|
|
|
|
|
|
|
# # std dev and average calculations |
|
|
|
ART = np.zeros((42, 3)) |
|
|
|
label_flag = False |
|
|
|
# Localize IndLoc data for each scaling factor in sf_list and each k in k_list |
|
|
|
sf_list = [0.001] # [0.002] |
|
|
|
k_list = [3] |
|
|
|
IndLoc_pos = calc_multiple_positions(data=IndLoc_data, |
|
|
|
output_filename="horizontal_diagonal", |
|
|
|
only_fs=True, |
|
|
|
sf_list=sf_list, |
|
|
|
k_list=k_list) |
|
|
|
|
|
|
|
x_list = [] |
|
|
|
y_list = [] |
|
|
|
z_list = [] |
|
|
|
# Find the best localization |
|
|
|
best_mean = 10000 |
|
|
|
best_std_dev = 10000 |
|
|
|
best_both = 10000 |
|
|
|
for i in range((len(sf_list)*len(k_list))): |
|
|
|
distances, curr_mean, curr_std_dev = eval_loc(ART_pos=ART_pos, IndLoc_pos=IndLoc_pos[i]) |
|
|
|
|
|
|
|
for i in range(1, 43): |
|
|
|
print("i", i) |
|
|
|
if i <= 21: |
|
|
|
pos = plot_art("horizontal_P" + str(i), annotate_point=False) |
|
|
|
else: |
|
|
|
if i == 42: |
|
|
|
label_flag = True |
|
|
|
pos = plot_art("diagonal_P" + str(i-21), annotate_point=False) |
|
|
|
x_list.append(pos[0]) |
|
|
|
y_list.append(pos[1]) |
|
|
|
z_list.append(pos[2]) |
|
|
|
|
|
|
|
ART[:, 0] = x_list |
|
|
|
ART[:, 1] = y_list |
|
|
|
ART[:, 2] = z_list |
|
|
|
|
|
|
|
|
|
|
|
# print("ART shape", np.shape(ART)) |
|
|
|
# print("ART nach dem laden:\n", ART) |
|
|
|
|
|
|
|
|
|
|
|
IndLoc = np.zeros((42, 3)) |
|
|
|
data = recording_dict_fs[recording_selection] |
|
|
|
|
|
|
|
scale_factor_list = [0.00204] #[0.001755, 0.001, 0.002, 0.003, 0.004, 0.005] |
|
|
|
print("Testing so many scaling_factors: ", len(scale_factor_list)) |
|
|
|
min_avg = 10000 |
|
|
|
min_std = 10000 |
|
|
|
min_both = 10000 |
|
|
|
|
|
|
|
best_sf = 0 |
|
|
|
mean_at_best_sf = 1000 |
|
|
|
std_at_best_sf = 1000 |
|
|
|
|
|
|
|
for s_f in scale_factor_list: |
|
|
|
localization.scale_factor = s_f |
|
|
|
localization.k_nearest = 3 |
|
|
|
IndLoc = localization.localize_all_samples_direct(data, "current_positions") |
|
|
|
|
|
|
|
# print("IndLoc shape", np.shape(IndLoc)) |
|
|
|
# print("IndLoc:\n", IndLoc) |
|
|
|
|
|
|
|
|
|
|
|
distances = np.zeros((42, 1)) |
|
|
|
for i in range(np.shape(IndLoc)[0]): |
|
|
|
x_IndLoc = IndLoc[i, 0] |
|
|
|
y_IndLoc = IndLoc[i, 1] |
|
|
|
z_IndLoc = IndLoc[i, 2] |
|
|
|
|
|
|
|
x_ART = ART[i, 0] |
|
|
|
y_ART = ART[i, 1] |
|
|
|
z_ART = ART[i, 2] |
|
|
|
|
|
|
|
distance = math.sqrt( (x_IndLoc - x_ART)**2 + (y_IndLoc - y_ART)**2 ) |
|
|
|
distances[i] = distance |
|
|
|
# print("i", i) |
|
|
|
# print("IndLoc: x=", x_IndLoc, ", y=", y_IndLoc) |
|
|
|
# print("ART: x=", x_ART, ", y=", y_ART) |
|
|
|
|
|
|
|
avg_distance = np.average(distances) |
|
|
|
median_distance = np.median(distances) |
|
|
|
mean_distance = np.mean(distances) |
|
|
|
std_dev_distance = np.std(distances) |
|
|
|
|
|
|
|
both = avg_distance + std_dev_distance |
|
|
|
|
|
|
|
if avg_distance < min_avg: |
|
|
|
min_avg = avg_distance |
|
|
|
# print("[mean]minimum found at scaling_factor = ", s_f, "; min_avg = ", min_both) |
|
|
|
if std_dev_distance < min_std: |
|
|
|
min_std = std_dev_distance |
|
|
|
# print("[std_dev]minimum found at scaling factor = ", s_f, "; min_std = ", min_std) |
|
|
|
if both < min_both: |
|
|
|
min_both = both |
|
|
|
# print("[both]scaling factor for lowest min_both = ", s_f, "; min_both = ", min_both) |
|
|
|
best_sf = s_f |
|
|
|
mean_at_best_sf = avg_distance |
|
|
|
std_at_best_sf = std_dev_distance |
|
|
|
|
|
|
|
print("Best scaling factor = ", best_sf, " mean = ", mean_at_best_sf, "std_dev = ", std_at_best_sf, "; min_both = ", min_both) |
|
|
|
|
|
|
|
# print("avg_distance", avg_distance) |
|
|
|
# print("median_distance", median_distance) |
|
|
|
# print("mean_distance", mean_distance) |
|
|
|
# print("std dev distance", std_dev_distance) |
|
|
|
|
|
|
|
calc_multiple_positions(recording_name=recording_selection, |
|
|
|
only_fs=True, |
|
|
|
sf_list=[best_sf], |
|
|
|
k_list=[3]) |
|
|
|
curr_both = curr_mean + curr_std_dev |
|
|
|
|
|
|
|
if curr_mean <= best_mean: |
|
|
|
best_both = curr_both |
|
|
|
best_mean = curr_mean |
|
|
|
best_std_dev = curr_std_dev |
|
|
|
index = i |
|
|
|
|
|
|
|
plot_multiple_positions(recording_name=recording_selection, |
|
|
|
best_sf = sf_list[index] |
|
|
|
|
|
|
|
print("\n----Best localization parameters were found!-----\n scaling_factor = ", best_sf, "\n mean_error = ", best_mean, |
|
|
|
"\n standard_deviation = ", best_std_dev) |
|
|
|
|
|
|
|
|
|
|
|
# Plot IndLoc pos |
|
|
|
plot_multiple_positions(positions_filename="horizontal_diagonal", |
|
|
|
sf_list=[best_sf], |
|
|
|
k_list=[3], |
|
|
|
k_list=k_list, |
|
|
|
only_label_once=True, |
|
|
|
xy_title="Testing the scaling factor for the entire localization area", |
|
|
|
yz_title="Testing the scaling factor for the entire localization area", |
|
|
|
xy_title="Statistical optimization of the scaling factor for the entire localization area", |
|
|
|
yz_title="Statistical optimization of the scaling factor for the entire localization area", |
|
|
|
annotate_points=False, |
|
|
|
labels=iter(["scaling factor = " + str(best_sf)]), |
|
|
|
color_all_black=True) |
|
|
|
|
|
|
|
|
|
|
|
annotationsize=15, |
|
|
|
labels=iter(["scaling factor = " + str("{:.6f}".format(best_sf))]), |
|
|
|
color_all_black=True, |
|
|
|
markersize=100) |
|
|
|
|
|
|
|
# Plot ART positions |
|
|
|
plot_art_positions(ART_pos, annotate_flag=False, markersize=100, annotation_size=15) |
|
|
|
|
|
|
|
|
|
|
|
# print(IndLoc_pos) |
|
|
|
#recording_selection = "horizontal_and_diagonal" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# scale_factor_list = [0.001755] |
|
|
|
# |
|
|
|
# calc_multiple_positions(recording_name=recording_selection, |
|
|
|
# only_fs=True, |
|
|
|
# sf_list=[0.001755], |
|
|
|
# k_list=[3]) |
|
|
|
# |
|
|
|
# plot_multiple_positions(recording_name=recording_selection, |
|
|
|
# sf_list=[0.001755], |
|
|
|
# k_list=[3], |
|
|
|
# only_label_once=True, |
|
|
|
# xy_title="Statistical optimization of the scaling factor for an offcenter set of points", |
|
|
|
# yz_title="Statistical optimization of the scaling factor for an offcenter set of points", |
|
|
|
# annotate_points=False, |
|
|
|
# labels=iter(["scaling factor = " + str("{:.6f}".format(0.001755))]), |
|
|
|
# color_all_black=True) |
|
|
|
|
|
|
|
xy_subplot.legend(bbox_to_anchor=(1.01, 1.00), loc=2, borderaxespad=0., fontsize=10) |
|
|
|
plt.show() |
seyffejn
3 years ago