BachelorThesis/BachelorThesis/Software/PyPrecisionGUI/plot_precision_tests.py

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 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(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'])
    #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])


# 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)

# 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)

# 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])

    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

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=k_list,
                        only_label_once=True,
                        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,
                        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()