from localization import Localization from plot_results import Plotting from ART import ART_plotting import matplotlib.pyplot as plt import numpy as np import re import math mm = 1000 # millimetre localization = Localization() plotting = Plotting() fig = plt.figure(figsize=[24, 9]) 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} plt.subplots_adjust(left=0.07, bottom=0.07, right=0.89, top=0.89, wspace=0.2, hspace=0.2) # Set Axis xy_subplot.set_xlim([-0.02 * mm, 0.52 * mm]) xy_subplot.set_ylim([-0.02 * mm, 0.52 * mm]) # xy_subplot.set_xticks(np.arange(0.0*mm, 0.55*mm, 0.05*mm)) # xy_subplot.set_yticks(np.arange(0.0*mm, 0.55*mm, 0.05*mm)) xy_subplot.set_xlabel('X [mm]', fontdict=font) xy_subplot.set_ylabel('Y [mm]', fontdict=font) xy_subplot.grid(alpha=0.6) # for label in xy_subplot.xaxis.get_ticklabels(): # label.set_rotation(90) # Set Axis yz_subplot.set_ylim([-0.02 * mm, 0.52 * mm]) yz_subplot.set_xlim([-0.02 * mm, 0.22 * mm]) # yz_subplot.set_ylim([0.1 * mm, 0.5 * mm]) # yz_subplot.set_xlim([0.0 * mm, 0.2 * 100]) # yz_subplot.set_yticks(np.arange(0.00 * mm, 0.5 * mm, 0.05 * mm)) # yz_subplot.set_xticks(np.arange(0.00 * mm, 0.20 * mm, 0.01 * mm)) xy_subplot.tick_params(labelsize=13) yz_subplot.tick_params(labelsize=13) yz_subplot.set_ylabel('Y [mm]', fontdict=font) yz_subplot.set_xlabel('Z [mm]', fontdict=font) yz_subplot.grid(alpha=0.5) # Set Titles xy_subplot.set_title("XY", fontdict=font) yz_subplot.set_title("ZY", fontdict=font) # CONFIGURE X, Y PLOT # Indicate Exciter Exciter = xy_subplot.vlines(x=0.0 * mm, ymin=0.0 * mm, ymax=0.5 * mm, linestyles="-", colors="#5b9bd5", label="Exciter", linewidth=6) xy_subplot.vlines(x=0.5 * mm, ymin=0.0 * mm, ymax=0.5 * mm, linestyles="-", colors="#5b9bd5", linewidth=6) xy_subplot.hlines(y=0.0 * mm, xmin=0.0 * mm, xmax=0.5 * mm, linestyles="-", colors="#5b9bd5", linewidth=6) xy_subplot.hlines(y=0.5 * mm, xmin=0.0 * mm, xmax=0.5 * mm, linestyles="-", colors="#5b9bd5", linewidth=6) yz_subplot.vlines(x=0.0, ymin=0.0 * mm, ymax=0.5 * mm, linestyles="-", colors="#5b9bd5", linewidth=6, alpha=0.3) # Indicate Localization Area # LocArea = xy_subplot.vlines(x=0.045, ymin=0.045, ymax=0.495, linestyles="dashed", colors="#7f7f7f", # label="Loc.\nArea") # xy_subplot.vlines(x=0.495, ymin=0.045, ymax=0.495, linestyles="dashed", colors="#7f7f7f") # xy_subplot.hlines(y=0.045, xmin=0.045, xmax=0.495, linestyles="dashed", colors="#7f7f7f") # xy_subplot.hlines(y=0.495, xmin=0.045, xmax=0.495, linestyles="dashed", colors="#7f7f7f") # SHOW ANTENNAS AntPosX = [0 * mm, 0 * mm, 0.13 * mm, 0.375 * mm, 0.5 * mm, 0.5 * mm, 0.125 * mm, 0.38 * mm] # Safing each Antennas Position in a list -> e.g. : X position of frame ant 1, X pos of frame ant 2, x pos of frame ant 3, ... AntPosY = [0.125 * mm, 0.375 * mm, 0.5 * mm, 0.5 * mm, 0.125 * mm, 0.375 * mm, 0 * mm, 0 * mm] AntOrientY = [270, 270, 180, 180, 90, 90, 0, 0] # analog as AntPosX,Y , just with Orientation degrees (direction in z-axis # Select Antenna plotting options here patchAlpha = 1.0 # ITERATE THROUGH CMAP------------- n = 8 # color = iter(cm.jet(np.linspace(0, 1, n))) # ---------------------------- for i in range(8): # iterate through antennas x = AntPosX[i] # get x and y pos of antennas y = AntPosY[i] # c = next(color) # Comment in for coloured antennas c = "k" # black antennas if i == 7: # only print one label for the Antennas (otherwise its 8 times in legend) label = label = str("Receiving\nCoils") else: label = "" if AntOrientY[i] == 0: # identify antennas orientation patchX = float(x - 0.033 * mm / 2) patchY = float(y - 0.033 * mm / 2) rect = plt.Rectangle((patchX, patchY), 0.033 * mm, 0.033 * mm, color=c, alpha=patchAlpha, label=label) xy_subplot.add_patch(rect) if AntOrientY[i] == 180: patchX = float(x - 0.033 * mm / 2) patchY = float(y - 0.033 * mm / 2) rect = plt.Rectangle((patchX, patchY), 0.033 * mm, 0.033 * mm, color=c, alpha=patchAlpha) xy_subplot.add_patch(rect) if AntOrientY[i] == 90: patchX = float(x - 0.033 * mm / 2) patchY = float(y - 0.033 * mm / 2) rect = plt.Rectangle((patchX, patchY), 0.033 * mm, 0.033 * mm, color=c, alpha=patchAlpha) xy_subplot.add_patch(rect) if AntOrientY[i] == 270: patchX = float(x - 0.033 * mm / 2) patchY = float(y - 0.033 * mm / 2) rect = plt.Rectangle((patchX, patchY), 0.033 * mm, 0.033 * mm, color=c, alpha=patchAlpha) xy_subplot.add_patch(rect) # Add antenna patches in yz plot # rect2 = plt.Rectangle((patchX, patchY), 0.033 * mm, 0.033 * mm, # color=c, alpha=0.3, label=label) # yz_subplot.add_patch(rect2) def art_file_to_position(recording_name): """ :param recording_name: int, Choose which recording to print e.g. Recording_!1! , Recording_!2!, etc. """ """--------------- Open, format and save ART positions from text files---------------------------------------""" # Opening a recording art_recording = open('ART Recordings (no ferrite)/' + str(recording_name) + '.drf', 'r') art_recording = art_recording.read() # Formatting a recording art_recording = re.split(' |\n', art_recording) # split string at whitespace and linebreak art_recording = np.array(art_recording) # list -> np.array # Count the total number of recorded positions total_pos_counter = 0 for i in art_recording: if i == '6d': # this is the flag marking a 6d position (Syntax of the ART guys in the txt files) total_pos_counter += 1 # Create an empty array which will store the ART measurement infos art_info = np.zeros((total_pos_counter, 7)) # [t, x, y, z, alpha, beta, gamma] # Fill the empty numpy array with the information's of the art_recording pos_counter = 0 for i in range(len(art_recording) - 9): if art_recording[i] == 'ts': # following entry is timestamp art_info[pos_counter] = float(art_recording[i + 1]) if art_recording[i] == '6d': # following entries are x,y,z,alpha,beta,gamma art_info[pos_counter, 1] = float(art_recording[i + 3].split('[')[1]) # x (had to delete some excess text) art_info[pos_counter, 2] = float(art_recording[i + 4]) # y art_info[pos_counter, 3] = float(art_recording[i + 5]) # z art_info[pos_counter, 4] = float(art_recording[i + 6]) # alpha art_info[pos_counter, 5] = float(art_recording[i + 7]) # beta art_info[pos_counter, 6] = float( art_recording[i + 8].split(']')[0]) # gamma (had to delete some excess text pos_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]] # 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 def plot_art_positions(positions, annotate_flag, markersize, annotation_size): 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 i == len(positions)-1: if annotate_flag: xy_subplot.annotate("P13", (positions[i, 0], positions[i, 1]), color="green", size=annotation_size) yz_subplot.annotate("P13", (positions[i, 2], positions[i, 1]), color="green", size=annotation_size) 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/..." :param recording_name: string name of recording (see recordings_dict) :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)] """ 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] 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 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-----") return positions 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']) markers = iter(['.', '.']) #colors = iter(["blue", "orange", "red"]) for sf in sf_list: for k in k_list: color = next(colors) marker = next(markers) 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=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=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=annotationsize) yz_subplot.annotate("P" + str(i + 1), (positions[i, 2], positions[i, 1]), color=color, size=annotationsize) yz_subplot.set_title(str(xy_title), size=20) 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. :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]) # Input here ----------------------------------------------------------------------------------------------------------- recording_selection = ["z_axis"] sf_list = [0.0021] # np.arange(0.0015, 0.0025, 0.0001) k_list = [12] annotate_points = False plot_title = "Noise investigation" label = "IndLoc" # End of input --------------------------------------------------------------------------------------------------------- # Make the plot look nice plot_surroundings() # Load data positions if len(recording_selection) == 1: # if we only have to look at simply load ART and IndLoc ART_pos = load_entire_ART(recording_selection[0]) # Load ART positions IndLoc_data = recording_dict_fs["z_axis"] # Load IndLoc data if len(recording_selection) == 2: # if we want to look at multiple recordings its a little more complicated ART_pos = load_entire_ART([recording_selection[0], recording_selection[1]]) # Load both ART positions rec1 = recording_dict_fs[recording_selection[0]] # Load IndLoc1 rec2 = recording_dict_fs[recording_selection[1]] # Load IndLoc2 IndLoc_data = np.append(rec1, rec2, axis=0) # Append both IndLoc arrays correctly # Average IndLoc data #IndLoc_data = np.average(IndLoc_data, axis=1) # shape (42, 19) = (P1-P42, fv_avg) # Localize IndLoc data for each scaling factor in sf_list and each k in k_list IndLoc_pos = calc_multiple_positions(data=IndLoc_data, output_filename=str(recording_selection), only_fs=True, sf_list=sf_list, k_list=k_list) # Find the best localization parameters # best_mean = 10000 # best_std_dev = 10000 # best_both = 10000 # for i in range((len(sf_list)*len(k_list))): # print(i) # distances, curr_mean, curr_std_dev = eval_loc(ART_pos=ART_pos, IndLoc_pos=IndLoc_pos[i]) # #print("- current k_nearest:", sf_list[i], " --> mean=", curr_mean, ", std_dev=",curr_std_dev) # curr_both = curr_mean + curr_std_dev # # if curr_both <= best_both: # best_both = curr_both # best_mean = curr_mean # best_std_dev = curr_std_dev # index = i # # if len(sf_list) != 1: # it looks like we`re analyzing multiple sf`s # best_k = k_list[0] # best_sf = sf_list[index] # if len(k_list) != 1: # best_sf = sf_list[0] # best_k = k_list[index] # if (len(sf_list) == 1) and (len(k_list) == 1): # best_sf = sf_list[0] # best_k = k_list[0] # # 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=str(recording_selection), sf_list=[0.001755], k_list=[19], only_label_once=True, xy_title=plot_title, yz_title=plot_title, annotate_points=annotate_points, annotationsize=15, labels=iter([label]), # labels=iter(["k = " + str(best_k)]), color_all_black=True, markersize=10,) # Plot ART positions plot_art_positions(ART_pos, annotate_flag=True, markersize=100, annotation_size=15) xy_subplot.legend(bbox_to_anchor=(1.01, 1.00), loc=2, borderaxespad=0., fontsize=10) plt.show()