Update main.py

main.py

+# From Python
+# It requires OpenCV installed for Python
+import sys
+import cv2
+import os
+from sys import platform
+import argparse
+import time
+import math
+import numpy as np
+import tensorflow as tf
+import numpy as np
+from tensorflow import keras
+import scipy.spatial as spatial
+import serial
+from multiprocessing import Process, Manager, Queue, Event
+from multiprocessing.connection import Client
+import statistics
+import subprocess
 
+#TODO remove
+import keras
+from keras.models import Model
+from keras.layers import Dense, Activation, Dropout, Input
+from keras.models import model_from_json
+from keras.layers import Concatenate, Add
+from keras import backend as K
+
+
+############################TO BE DELETED ##########################################
+def multi():
+    input1 = Input(shape=(30,))
+
+    x = Dense(30)(input1)
+    x = Activation("tanh")(x)
+    x = Dropout(0.20)(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dropout(0.20)(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(15)(x)
+    x = Activation("tanh")(x)
+
+    x = Concatenate()([input1, x])
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dropout(0.10)(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(15)(x)
+    x = Activation("tanh")(x)
+
+    x = Concatenate()([input1, x])
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dropout(0.10)(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(15)(x)
+    x = Activation("tanh")(x)
+
+    x = Concatenate()([input1, x])
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dropout(0.10)(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(15)(x)
+    x = Activation("tanh")(x)
+
+    x = Concatenate()([input1, x])
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dense(45)(x)
+    x = Activation("tanh")(x)
+    x = Dropout(0.10)(x)
+    x = Dense(30)(x)
+    x = Activation("tanh")(x)
+    x = Dense(15)(x)
+    final = Activation("tanh")(x)
+
+    model = Model(inputs=input1, outputs=final)
+    return model
+
+
+############################# GLOBAL VARIABLES ######################################
+command_port1 = 'COM6'
+data_port1 = 'COM7'
+command_port2 = None
+data_port2 = None
+command_port3 = None
+data_port3 = None
+
+##############################RADAR SESSION CLASS#####################################
+
+class RadarSession:
+
+    CONFIG_FILE = 'mmw_pplcount_demo_default.cfg'
+    TIMEOUT = 0.05  # TODO RESET TO 0.05
+    POINT_CLOUD_TLV = 6
+    TARGET_OBJECT_TLV = 7
+    TARGET_INDEX_TLV = 8
+    MAX_BUFFER_SIZE = 2 ** 15
+    TLV_HEADER_BYTES = 8
+    TARGET_LIST_TLV_BYTES = 68
+    POINT_CLOUD_TLV_POINT_BYTES = 16
+    MAGIC_WORD = [2, 1, 4, 3, 6, 5, 8, 7]
+    # word array to convert 4 bytes to a 32 bit number
+    WORD = [1, 2 ** 8, 2 ** 16, 2 ** 24]
+
+    def __init__(self, command_port, data_port):
+        self.CLIport, self.Dataport = self.serialConfig(command_port, data_port)
+
+    # Function to configure the serial ports and send the data from
+    # the configuration file to the radar
+    def serialConfig(self, command_port, data_port):
+        # Open the serial ports for the configuration and the data ports
+        CLIport = serial.Serial(command_port, 115200)
+        Dataport = serial.Serial(data_port, 921600, timeout=self.TIMEOUT)
+        # Read the configuration file and send it to the board
+        config = [line.rstrip('\r\n') for line in open(self.CONFIG_FILE)]
+        for i in config:
+            CLIport.write((i + '\n').encode())
+            print(i)
+            time.sleep(0.01)
+        return CLIport, Dataport
+
+    # Funtion to read and parse the incoming data
+    def readAndParseData(self):
+        readBuffer = self.Dataport.read(
+            size=self.MAX_BUFFER_SIZE - 1)  # TODO inside brackets it was Dataport.in_waiting
+        byteVec = np.frombuffer(readBuffer, dtype='uint8')
+        byteBufferLength = len(byteVec)
+        # Check for all possible locations of the magic word
+        possibleLocs = np.where(byteVec == self.MAGIC_WORD[0])[0]
+        # Confirm that it is the beginning of the magic word and store the index in startIdxes
+        startIdxes = []
+        for loc in possibleLocs:
+            check = byteVec[loc:loc + 8]
+            if np.all(np.array_equal(check, self.MAGIC_WORD)):
+                startIdxes.append(loc)
+        for startIdx in startIdxes:
+            # Read the total packet length
+            totalPacketLen = np.matmul(byteVec[(startIdx + 20):(startIdx + 20 + 4)], self.WORD)
+            # Check that all the packet has been read
+            if (startIdx + totalPacketLen) <= byteBufferLength:
+                # Initialize the pointer index
+                idX = startIdx + 48
+                numTLVs = np.matmul(byteVec[idX:idX + 2], self.WORD[0:2])
+                idX += 4
+                # If the number of TLV messages is at most 3
+                if 0 < numTLVs <= 3:
+                    # Read the TLV messages
+                    for tlvIdx in range(numTLVs):
+                        # Check the header of the TLV message
+                        tlv_type = np.matmul(byteVec[idX:idX + 4], self.WORD)
+                        idX += 4
+                        # Read the data depending on the TLV message
+                        if tlv_type == self.POINT_CLOUD_TLV:
+                            tlv_length = np.matmul(byteVec[idX:idX + 4], self.WORD)
+                            idX += (tlv_length - 4)
+                        # Read the data depending on the TLV message
+                        if tlv_type == self.TARGET_OBJECT_TLV:
+                            tlv_length = np.matmul(byteVec[idX:idX + 4], self.WORD)
+                            idX += 8  # +8 because i do not need the TID
+                            number_of_targets = int((tlv_length - self.TLV_HEADER_BYTES) / self.TARGET_LIST_TLV_BYTES)
+
+                            # Initialize array of 2D coordinates
+                            xy = np.zeros((number_of_targets, 2), dtype=np.float32)
+                            for target in range(number_of_targets):
+                                # Read the data for each target
+                                xy[target, 0] = byteVec[idX:idX + 4].view(dtype=np.float32)
+                                idX += 4
+                                xy[target, 1] = byteVec[idX:idX + 4].view(dtype=np.float32)
+                                idX += 64
+                            return xy
+        return None
+
+############################## UTILITY FUNCTIONS #####################################
+
+#This function opens a radar session at a specific command and data port, giving the radar session an id, and
+# continuously waits on an event to capture data from the radar. The output of the radr is fed into a queue.
+def radar_session(id, command_port, data_port, event, queue, *args):
+    try:
+        session = RadarSession(command_port, data_port)
+        while True:
+            event.wait()
+            xy = session.readAndParseData()
+            queue.put((id, xy))
+            event.clear()
+    except:
+        print("Radar " + id + "session cannot be started")
+
+
+#This function takes as input an array of 2d coordinates detected by the three radars under the same reference frame and
+# returns an array of the corresponding x angle and euclidean distance
+def from_2d_to_angles_euclid(coords):
+    # coords shape = (N,2) where N=detected people, 2=(x,z)
+    angles_euclid = np.zeros((coords.shape[0], 2), dtype=np.float64)
+    # angles_euclid shape = (N,2) where N=detected people, 2=(x-angle, euclidean distance)
+    angles_euclid[:, 1] = np.sqrt(np.sum(coords ** 2, axis=1))
+    angles_euclid[:, 0] = np.arccos(coords[:, 0] / angles_euclid[:, 1]) * np.sign(coords[:, 1])
+    return angles_euclid
+
+
+#This function takes as input the 2D coordinates of the people detected by the three radars and converts them into the
+#same reference frame, which is the reference frame of the first radar
+def under_same_reference(coords1, coords2, coords3):
+    # coords1 shape = (N,2) where N=people detected, 2=(x,y)
+    # coords2 shape = (T,2) where T=people detected, 2=(x,y)
+    # coords3 shape = (S,2) where S=people detected, 2=(x,y)
+    radian2, radian3 = np.radians(120), np.radians(-120)
+    coords2 = np.dot(np.asarray([[np.cos(radian2), -np.sin(radian2)], [np.sin(radian2), np.cos(radian2)]]), coords2.T).T
+    # coords2 shape = (T,2) where T=people detected, 2=(x,y)
+    coords3 = np.dot(np.asarray([[np.cos(radian3), -np.sin(radian3)], [np.sin(radian3), np.cos(radian3)]]), coords3.T).T
+    # coords3 shape = (S,2) where T=people detected, 2=(x,y)
+    output = np.concatenate((coords1, coords2, coords3), axis=0)
+    # output shape = (N+T+S, 2)
+    return output
+
+
+def from_image_space_to_angle(pixel_keypoints, width, height):
+    # pixel_keypoints shape = (N, 15, 2) where N=people detected, 15=human body keypoints, 2=(x,y)
+    pixel_keypoints[:,:,0] = (pixel_keypoints[:,:,0] - (width/2)) * ((2. * np.pi) / width)
+    pixel_keypoints[:,:,1] = ((height / 2.) - pixel_keypoints[:,:,1]) * (np.pi / height)
+    x_angles = pixel_keypoints[:,:,0]
+    # convert camera angles into angles relative to x-axis
+    pixel_keypoints[:,:,0] = np.where(((x_angles >= -np.pi/2) & (x_angles <= np.pi)),  np.pi/2 - x_angles,
+                                      -np.pi + (-x_angles - np.pi/2))
+    return pixel_keypoints
+
+
+# This function takes as input the x and y angles of each body keypoint relative to the camera, the euclidean distance
+# of the subject from the camera as measured by the radar in metric units and outputs real x and y coordinates + a
+# general depth z.
+def coordinates_from_angles(dataset, euclid_distance, camera_height):
+  # dataset shape = (N, 15, 2) where N=frames, 15=human body keypoints, 2=(x-angle,y-angle)
+  # euclidean_distance shape = (N,1) where N=frames
+  # camera_height = double
+  x = np.cos(dataset[:,:,0]) * euclid_distance
+  # x shape = (N,15) where N=frames, 15=human body keypoints
+  y = camera_height + np.tan(dataset[:,:,1]) * euclid_distance
+  # y shape = (N,15) where N=frames, 15=human body keypoints
+  z = np.sin(dataset[:,:,0]) * euclid_distance
+  # z shape = (N,15) where N=frames, 15=human body keypoints
+  output = np.concatenate((np.expand_dims(x,2), np.expand_dims(y,2), np.expand_dims(z,2)), axis = 2)
+  # output shape = (N, 15, 3) where N=frames, 15=human body keypoints, 3=(x,y,z)
+  return output
+
+
+# This function takes as input the x and y angles of each body keypoint relative to the camera, the angle and
+# euclidean distance of the subjects detected from the radar, which need to be converted in relation to the camera before
+# calling this function, and matches each subject detected from the camera to its euclidean distance measured by the radar
+def match_euclidean_distances_to_subjects(keypoint_angles, radar_data):
+    # keypoint_angles shape = (N, 15, 2) where N=people detected, 15=human body keypoints, 2=(x-angle,y-angle)
+    # radar_data shape = (N, 2) where N=people detected, 2=(x-angle, euclidean distance)
+    keypoint_mean_angles = np.nanmean(keypoint_angles, axis=1)
+    # keypoint_mean_angles shape = (N,2) where N=people detected, 2=(x-angle, y-angle)
+    keypoint_mean_angles = np.expand_dims(keypoint_mean_angles[:, 0], 1)
+    # keypoint_mean_angles shape = (N,1) where N=people detected
+    people_detected = keypoint_mean_angles.shape[0]
+    queries = radar_data[:, 0].tolist()
+    tree = spatial.KDTree(keypoint_mean_angles)
+    indexes = []
+    for query in queries:
+        for p in range(people_detected):
+            d, index = tree.query([query], k=p+1)
+            if isinstance(index, np.int32):
+                if index not in indexes:
+                    indexes.append(index)
+                    break
+            else:
+                if index[-1] not in indexes:
+                    indexes.append(index[-1])
+                    break
+    not_detected_indexes = [i for i in range(people_detected) if i not in indexes]
+    return keypoint_angles[indexes], np.expand_dims(radar_data[:, 1], axis=1), keypoint_angles[not_detected_indexes]
+
+
+# Function that normalises keypoints
+def normalize_keypoints(keypoints):
+    """
+    Calculate mean for x, y (and z if present), and standard deviation for x and y in given frame.
+    Subtract respective mean from each point, then divide by avg of std of x and y.
+    """
+    # keypoints shape = (N, 15, 3) or (N, 15, 2) where N = number of people detected, 15=depth estimates for each human skeleton keypoint
+    means = np.mean(keypoints, axis = 1)
+    # means shape = (N,3) or (N,2) where N = number of people detected
+    denoms = np.sum(np.std(keypoints[:,:,0:2], axis=1), axis=1, keepdims=True) / 2
+    # denoms shape = (N,1) where N = number of people detected
+    norm_keypoints = (keypoints - np.expand_dims(means, axis=1)) / np.expand_dims(denoms, axis=1)
+    # norm_keypoints shape = (N, 15, 3) where N = number of people detected, 15=depth estimates for each human skeleton keypoint
+    return norm_keypoints, means, denoms
+
+
+# Function that unnormalises depth
+def unnormalize_depth(norm_depths, average_depths, denoms):
+  # norm_depths shape = (N,15) where N = number of people detected, 15=depth estimates for each human skeleton keypoint
+  # average_depths shape = (N,1) where N = number of people detected
+  # denoms shape = (N,1) where N = number of people detected
+  return norm_depths * denoms + average_depths
+
+
+# Funtion that takes as input a numpy array of people 2d keypoints and their (general) depth and outputs their 3d poses.
+# The general depth is computed with the radar distance.
+# The coordinates provided in the keypoints array are in metric units.
+def threeDposes(keypoints, model):
+  # keypoints shape = (N,15,3) where N=number of people detected, 15=human skeleton keypoints, 3=(x,y,z)
+  norm_keypoints, means, denoms = normalize_keypoints(keypoints / 0.0564) # keypoints are first converted into inches units
+  input = norm_keypoints[:,:,0:2]
+  input = input.reshape(input.shape[0], input.shape[1] * input.shape[2])
+  norm_depths = model(input) #.predict() is just good for large batch execution
+  # norm_depths shape = (N,15) where N = number of people detected, 15=depth estimates for each human skeleton keypoint
+  unnorm_depths = unnormalize_depth(norm_depths, np.expand_dims(means[:,2], axis=1), denoms) * 0.0564 # convert into metric units
+  # unnorm_depths shape = (N, 15) where N = number of people detected, 15=depth estimates for each human skeleton keypoint
+  unnorm_depths = np.expand_dims(unnorm_depths, 2)
+  # unnorm_depths shape = (N, 15, 1) where N = number of people detected, 15=depth estimates for each human skeleton keypoint
+  output = np.concatenate((keypoints[:,:,0:2], unnorm_depths), axis = 2)
+  # output shape = (N, 15, 3) where N=number of people detected, 15=human skeleton keypoints, 3=(x,y,z)
+  return output
+
+def skeleton_poses(skeletons):
+  N, P, L = skeletons.shape
+  # skeletons shape = (N, 15, 3) where N=people, 15=human body keypoints, 3=(x,y,z)
+  skeletons = np.reshape(skeletons, newshape=(N*P, L))
+  # skeletons shape = (N*15, 3) where N=people, 15=human body keypoints, 3=(x,y,z)
+  lines = np.asarray([[0,1], [1,2], [2,3], [3,4], [1,5], [5,6], [6,7], [8,9], [9,10], [11,12], [12,13], [1,14], [8,14], [11,14]], dtype=int)
+  # lines shape = (14, 2)
+  lines = np.expand_dims(lines, axis=0)
+  # lines shape = (1, 14, 2)
+  lines = np.repeat(lines, repeats=N, axis=0)
+  # lines shape = (N, 14, 2)
+  summation = np.expand_dims(np.expand_dims(np.arange(0, 15*(N-1)+1, 15, dtype=int), axis=1), axis=1)
+  # summation shape = (N, 1, 1)
+  lines = lines + summation
+  # lines shape = (N, 14, 2)
+  lines = np.reshape(lines, (lines.shape[0]*lines.shape[1], lines.shape[2]))
+  # lines shape = (N*14, 2)
+  lines = o3d.utility.Vector2iVector(lines)
+  line_set = o3d.geometry.LineSet()
+  line_set.lines = lines
+  line_set.points = o3d.utility.Vector3dVector(skeletons)
+  return line_set
+
+
+def gui(queue, *args):
+    # CREATION OF THE NON-BLOCKING OPEN3D SCENE
+    vis = o3d.visualization.Visualizer()
+    vis.create_window()
+
+    line_set = o3d.geometry.LineSet()
+
+    mesh_box = o3d.geometry.TriangleMesh.create_box(width=40.0,
+                                                    height=0.1,
+                                                    depth=40.0)
+    mesh_box.paint_uniform_color([0.8, 0.8, 0.8])
+    mesh_box.translate(-mesh_box.get_center())
+    mesh_box.translate([0, -0.2, 0])
+
+    vis.add_geometry(line_set)
+    vis.add_geometry(mesh_box)
+
+    keyboard.send('alt', True, False)
+
+    ctr = vis.get_view_control()
+    parameters = o3d.io.read_pinhole_camera_parameters("ScreenCamera_2022-11-25-12-27-51.json")
+    ctr.convert_from_pinhole_camera_parameters(parameters)
+
+    axis = o3d.geometry.LineSet()
+    axis_points = o3d.utility.Vector3dVector(
+        np.asarray([[0, 0, 0], [0, 20, 0], [-20, 0, 0], [20, 0, 0], [0, 0, -20], [0, 0, 20]]))
+    axis_lines = o3d.utility.Vector2iVector(np.asarray([[0, 1], [2, 3], [4, 5]]))
+    axis.points = axis_points
+    axis.lines = axis_lines
+
+    vis.add_geometry(axis)
+
+    while (True):
+        if not queue.empty():
+            # vis.clear_geometries()
+            poses = queue.get()
+            # poses shape = (N, 15, 3) where N=people detected, 15=human body keypoints, 3=(x,y,z)
+            updated_line_set = skeleton_poses(poses)
+            line_set.lines = updated_line_set.lines
+            line_set.points = updated_line_set.points
+
+            vis.update_geometry(line_set)
+
+        if keyboard.is_pressed('q'):
+            vis.destroy_window()
+            keyboard.send('alt', False, True)
+
+        vis.poll_events()
+        vis.update_renderer()
+
+        print("FPS: ", 1.0 / (time.time() - start_time))  # TESTING
+
+
+######################################## MAIN THREAD ###################################################
+
+if __name__ == '__main__':
+
+    ############################### START GUI PROCESS #######################################
+    subprocess.Popen("C:/Users/at2n19/.conda/envs/visualiser/python.exe gui.py")
+    time.sleep(5)
+    address = ('localhost', 6000)
+    conn = Client(address, authkey=None)
+    #q_poses = Queue()
+    #gui_process = Process(target=gui, args=(q_poses,))
+    #gui_process.start()
+    ############################### START RADAR SESSIONS ####################################
+    radar_event = Event()
+    radar_queue = Queue()
+    radar_process1 = Process(target=radar_session, args=(1, command_port1, data_port1, radar_event, radar_queue,))
+    radar_process2 = Process(target=radar_session, args=(2, command_port2, data_port2, radar_event, radar_queue,))
+    radar_process3 = Process(target=radar_session, args=(3, command_port3, data_port3, radar_event, radar_queue,))
+    radar_process1.start()
+    radar_process2.start()
+    radar_process3.start()
+
+    ##################### NEURAL NETWORK MODEL INSTANTIATION ################################
+    
+    gpus = tf.config.experimental.list_physical_devices('GPU')
+    for gpu in gpus:
+        tf.config.experimental.set_memory_growth(gpu, True)
+
+    model = multi() # todo remove
+    model.load_weights("part_trained.hdf5") #TODO REMOVE
+
+    #model = keras.models.load_model("fully_trained_model.hdf5") # TO SET BACK
+
+    ################################### MAIN CODE #################################################
+    cap = None
+    try:
+        # Import Openpose (Windows/Ubuntu/OSX)
+        dir_path = os.path.dirname(os.path.realpath(__file__))
+        try:
+            # Windows Import
+            if platform == "win32":
+                # Change these variables to point to the correct folder (Release/x64 etc.)
+                sys.path.append(dir_path + '/../../python/openpose/Release')
+                os.environ['PATH']  = os.environ['PATH'] + ';' + dir_path + '/../../x64/Release;' +  dir_path + '/../../bin;'
+                import pyopenpose as op
+            else:
+                # Change these variables to point to the correct folder (Release/x64 etc.)
+                sys.path.append('../../python')
+                # If you run `make install` (default path is `/usr/local/python` for Ubuntu), you can also access the OpenPose/python module from there. This will install OpenPose and the python library at your desired installation path. Ensure that this is in your python path in order to use it.
+                # sys.path.append('/usr/local/python')
+                from openpose import pyopenpose as op
+        except ImportError as e:
+            print('Error: OpenPose library could not be found. Did you enable `BUILD_PYTHON` in CMake and have this Python script in the right folder?')
+            raise e
+
+        # Flags
+        parser = argparse.ArgumentParser()
+        parser.add_argument("--image_dir", default="../../../examples/media/", help="Process a directory of images. Read all standard formats (jpg, png, bmp, etc.).")
+        parser.add_argument("--no_display", default=False, help="Enable to disable the visual display.")
+        parser.add_argument("--camera_height", type=float, default=1.45, help="Set camera height")
+        args = parser.parse_known_args()
+
+        # Custom Params (refer to include/openpose/flags.hpp for more parameters)
+        params = dict()
+        params["model_folder"] = "../../../models/"
+
+        # Add others in path?
+        for i in range(0, len(args[1])):
+            curr_item = args[1][i]
+            if i != len(args[1])-1: next_item = args[1][i+1]
+            else: next_item = "1"
+            if "--" in curr_item and "--" in next_item:
+                key = curr_item.replace('-','')
+                if key not in params:  params[key] = "1"
+            elif "--" in curr_item and "--" not in next_item:
+                key = curr_item.replace('-','')
+                if key not in params: params[key] = next_item
+
+        # Construct it from system arguments
+        # op.init_argv(args[1])
+        # oppython = op.OpenposePython()
+
+        cap = cv2.VideoCapture(1)
+
+        # Starting OpenPose
+        opWrapper = op.WrapperPython()
+        opWrapper.configure(params)
+        opWrapper.start()
+
+        frameId = 0 #todo delete then
+        time.sleep(10)
+        print("NOWWWWWWW")
+        print("NOWWWWWWWW")
+
+        while True:
+            # Capture video frame from camera
+            ret, frame = cap.read()
+            coords = np.asarray([[0., 2.]]) #TODO DELETE THEN
+            radar_data = from_2d_to_angles_euclid(coords) #TODO DELETE THEN
+            height, width, _ = frame.shape #TODO DELETE THEN
+            datum = op.Datum() #TODO DELETE THEN
+            datum.cvInputData = frame #TODO DELETE THEN
+            opWrapper.emplaceAndPop(op.VectorDatum([datum])) #TODO DELET THEN
+            cv2.imwrite("imageTrial.png", datum.cvOutputData)
+            # # Capture data from radars
+            # radar_event.set()
+            # xzs = []
+            # for i in range(3):
+            #     xzs.append(radar_queue.get())
+            # xzs.sort(key=lambda x: x[0])
+            # xzs = map(lambda x: x[1], xzs)
+            # coords = under_same_reference(xzs[0], xzs[1], xzs[2])
+            # radar_data = from_2d_to_angles_euclid(coords)
+            # height, width, _ = frame.shape
+            # datum = op.Datum()
+            # datum.cvInputData = frame
+            # opWrapper.emplaceAndPop(op.VectorDatum([datum]))
+            # cv2.imshow('frame', datum.cvOutputData)
+            # if cv2.waitKey(1) & 0xFF == ord('q'):
+            #     break
+            if datum.poseKeypoints is not None:
+                pixel_keypoints = np.round(datum.poseKeypoints[:, [0,1,5,6,7,2,3,4,12,13,14,9,10,11,8], :2]) # todo remove
+                # todo uncomment pixel_keypoints = np.round(datum.poseKeypoints[:,[0,1,2,3,4,5,6,7,9,10,11,12,13,14,8],:2]) # convert tp descrete pixel coordinates while still maintaining double type
+                # datum.poseKeypoints shape = (N, 25, 3) where N=people detected by camera, 25=human body keypoints, 3=(x,y,score)
+                # pixel_keypoints shape = (N, 15, 2) where N=people detected by camera, 15=human body keypoints, 2=(x,y)
+                # replace not detected keypoints (they have 0 pixel coordinates) with nan
+                pixel_keypoints[pixel_keypoints[:, :] == [0., 0.]] = np.nan
+                keypoints_angles = from_image_space_to_angle(pixel_keypoints, width, height)
+                # keypoints_angles shape = (N, 15, 2) where N=people detected by camera, 15=human body keypoints, 2=(x,y)
+                keypoints_angles, euclid_distance, not_matched_keypoints_angles = match_euclidean_distances_to_subjects(keypoints_angles, radar_data)
+                # keypoints_angles shape = (P, 15, 2) where P=people detected by camera and radar, 15=human body keypoints, 2=(x,y)
+                # eculid_distance shape = (P,1) where P=people detected by camera and radar, 15=human body keypoints, 2=(x,y)
+                # not_matched_keypoints_angles shape = (L, 15, 2) where P=people detected by camera and not radar, 15=human body keypoints, 2=(x,y)
+                estimated_3D_coordinates = coordinates_from_angles(keypoints_angles, euclid_distance, args[0].camera_height)
+                # estimated_3D_coordinates shape = (P, 15, 3) where P=people detected by camera and radar, 15=human body keypoints, 3=(x,y,z)
+                no_occlusion_keypoints_indexes = np.all(np.all(~np.isnan(estimated_3D_coordinates), axis=1), axis=1)
+                no_occlusion_estimated_3D_coordinates = estimated_3D_coordinates[no_occlusion_keypoints_indexes]
+                # no_occlusion_estimated_3D_coordinates shape = (T, 15, 3)  where T= not occluded people detected by camera and radar, 15=human body keypoints, 3=(x,y,z)
+                neural_net_3D_poses = threeDposes(no_occlusion_estimated_3D_coordinates, model)
+                # neural_net_3D_poses shape = (T, 15, 3)  where T= not occluded people detected by camera and radar, 15=human body keypoints, 3=(x,y,z)
+                occlusion_keypoints_indexes = ~ no_occlusion_keypoints_indexes
+                occlusion_estimated_3D_coordinates = estimated_3D_coordinates[occlusion_keypoints_indexes]
+                # average the result from the neural network and the estimated 3d poses
+                neural_net_3D_poses[:,:,2] = (neural_net_3D_poses[:,:,2] + no_occlusion_estimated_3D_coordinates[:,:,2])/2
+                # occlusion_estimated_3D_coordinates shape = (S, 15, 3)  where S = occluded people detected by camera and radar, 15=human body keypoints, 3=(x,y,z)
+                output_poses = np.concatenate((neural_net_3D_poses, occlusion_estimated_3D_coordinates), axis=0)
+                # output_poses shape = (P, 15, 3) where P=people detected by camera and radar, 15=human body keypoints, 3=(x,y,z)
+                # Add poses to the queue for the GUI
+                #np.save(("frame"+str(frameId)+".npy"), output_poses) #todo delete then
+                #cv2.imwrite(("frame"+str(frameId)+".png"), frame) #todo delete then
+                #cv2.imwrite(("frameDatum"+str(frameId)+".png"), datum.cvOutputData) #todo delete then
+                #frameId+=1 #todo delete then
+                #q_poses.put(output_poses) todo delete then
+                cv2.imshow('frame', frame)
+                if cv2.waitKey(1) & 0xFF == ord('q'):
+                    cap.release()
+                    conn.send('close')
+                    conn.close()
+                    break
+                conn.send(output_poses)
+
+    except Exception as e:
+        print(e)
+        cap.release()
+        conn.send('close')
+        conn.close()
+        sys.exit(-1)