def _load_file(self): ob = [] with open(self.TEST_FILE) as csvfile: reader = csv.DictReader(csvfile) for idx,row in enumerate(reader): ob.append(Object_State( name=row['agent1'], timestamp=idx+1, x=float(row['x1']), y=float(row['y1']) )) ob.append(Object_State( name=row['agent2'], timestamp=idx+1, x=float(row['x2']), y=float(row['y2']) )) ob.append(Object_State( name=self.__duplicate, timestamp=idx+1, x=float(row['x2']), y=float(row['y2']) )) self.world.add_object_state_series(ob)
def __compute_qsr(self, bb1, bb2): if not self.qsr: return "" ax, ay, bx, by = self.bb1 cx, cy, dx, dy = self.bb2 qsrlib = QSRlib() world = World_Trace() world.add_object_state_series([ Object_State(name="red", timestamp=0, x=((ax + bx) / 2.0), y=((ay + by) / 2.0), xsize=abs(bx - ax), ysize=abs(by - ay)), Object_State(name="yellow", timestamp=0, x=((cx + dx) / 2.0), y=((cy + dy) / 2.0), xsize=abs(dx - cx), ysize=abs(dy - cy)) ]) dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}} qsrlib_request_message = QSRlib_Request_Message( which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args) qsrlib_response_message = qsrlib.request_qsrs( req_msg=qsrlib_request_message) for t in qsrlib_response_message.qsrs.get_sorted_timestamps(): foo = "" for k, v in zip( qsrlib_response_message.qsrs.trace[t].qsrs.keys(), qsrlib_response_message.qsrs.trace[t].qsrs.values()): foo += str(k) + ":" + str(v.qsr) + "; \n" return foo
def getQSRState(self, xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1, td): isValid = False state = None human_os = [ Object_State(name="human_os", timestamp=0, x=xh0, y=yh0), Object_State(name="human_os", timestamp=td, x=xh1, y=yh1) ] robot_os = [ Object_State(name="robot_os", timestamp=0, x=xr0, y=yr0), Object_State(name="robot_os", timestamp=td, x=xr1, y=yr1) ] # make some input data world = World_Trace() world.add_object_state_series(human_os) world.add_object_state_series(robot_os) # make a QSRlib request message qsrlib_request_message = QSRlib_Request_Message( self.which_qsr, world, self.dynammic_args) # request your QSRs qsrlib_response_message = self.qsrlib.request_qsrs( req_msg=qsrlib_request_message) # should have 1 timestamp t = qsrlib_response_message.qsrs.get_sorted_timestamps() if len(t) != 1: rospy.logerr( "Node [" + rospy.get_name() + "@getQSRState] : response timestamp message lenght is not 1.") return (isValid, state) t = t[0] # should have one value: human to robot v = qsrlib_response_message.qsrs.trace[t].qsrs.values() if len(v) != 1: rospy.logerr( "Node [" + rospy.get_name() + "@getQSRState] : response values message lenght is not 1.") return (isValid, state) v = v[0] state = v.qsr.values() if len(state) != 1: rospy.logerr( "Node [" + rospy.get_name() + "@getQSRState] : response state message lenght is not 1.") return (isValid, state) state = state[0] # Ok, maybe is valid ... isValid = True return (isValid, state)
def apply_mean_filter(self, window_length=3): """Once obtained the joint x,y,z coords. Apply a median filter over a temporal window to smooth the joint positions. Whilst doing this, create a world Trace object""" joints, ob_states = {}, {} world = World_Trace() window = {} filtered_cnt = 0 for t in self.sorted_timestamps: joints[t] = {} for joint_id, (x, y, z) in self.skeleton_data[t].items(): # print "jointID=", joint_id, (x,y,z) try: window[joint_id].pop(0) except (IndexError, KeyError): window[joint_id] = [] window[joint_id].append((float(x), float(y), float(z))) avg_x, avg_y, avg_z = 0, 0, 0 for l, point in enumerate(window[joint_id]): avg_x += point[0] avg_y += point[1] avg_z += point[2] x, y, z = avg_x / float(l + 1), avg_y / float( l + 1), avg_z / float(l + 1) joints[t][joint_id] = (x, y, z) #Create a QSRLib format list of Object States (for each joint id) if joint_id not in ob_states.keys(): ob_states[joint_id] = [ Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z) ] else: ob_states[joint_id].append( Object_State(name=joint_id, timestamp=filtered_cnt, x=x, y=y, z=z)) filtered_cnt += 1 # #Add all the joint IDs into the World Trace for joint_id, obj in ob_states.items(): world.add_object_state_series(obj) self.filtered_skeleton_data = joints self.camera_world = world
def isValidState(xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1, td, which_qsr, dynammic_args, forbidden_states): ans = 1 human_os = [ Object_State(name="human_os", timestamp=0, x=xh0, y=yh0), Object_State(name="human_os", timestamp=td, x=xh1, y=yh1) ] robot_os = [ Object_State(name="robot_os", timestamp=0, x=xr0, y=yr0), Object_State(name="robot_os", timestamp=td, x=xr1, y=yr1) ] # make some input data world = World_Trace() world.add_object_state_series(human_os) world.add_object_state_series(robot_os) # make a QSRlib request message qsrlib_request_message = QSRlib_Request_Message(which_qsr, world, dynammic_args) # request your QSRs qsrlib_response_message = qsrlib.request_qsrs( req_msg=qsrlib_request_message) # should have 1 timestamp t = qsrlib_response_message.qsrs.get_sorted_timestamps() if len(t) != 1: print("something is wrong!!!!!!!!!!!!! with t") sys.exit(1) t = t[0] # should have one value: human to robot v = qsrlib_response_message.qsrs.trace[t].qsrs.values() if len(v) != 1: print("something is wrong!!!!!!!!!!!!! with v") sys.exit(1) v = v[0] state = v.qsr.values() if len(state) != 1: print("something is wrong!!!!!!!!!!!!! with state") sys.exit(1) state = state[0] if state in forbidden_states: ans = 0 #print(state) return (ans, state)
def relations_callback(self, extracted_pose1, extracted_pose2): o1 = [ Object_State(name="object_1", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose1.pose_vec.position.x, y=extracted_pose1.pose_vec.position.z) ] o2 = [ Object_State(name="robot", timestamp=extracted_pose1.header.stamp.secs, x=0, y=0) ] o3 = [ Object_State(name="object_2", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose2.pose_vec.position.x, y=extracted_pose2.pose_vec.position.z) ] self.world.add_object_state_series(o1) self.world.add_object_state_series(o2) self.world.add_object_state_series(o3) qsrlib_request_message = QSRlib_Request_Message( which_qsr=self.calculi, input_data=self.world, dynamic_args=self.dynamic_args) req = self.cln.make_ros_request_message(qsrlib_request_message) res = self.cln.request_qsrs(req) qsrlib_response_message = pickle.loads(res.data) argd_relation_msg = QsrMsg() argd_relation_msg.selected_qsr = str(self.calculi) argd_relation_msg.marker_id1 = extracted_pose1.marker_id argd_relation_msg.marker_id2 = extracted_pose2.marker_id for time in qsrlib_response_message.qsrs.get_sorted_timestamps(): argd_relation_msg.message_time = extracted_pose1.header.stamp.secs object_key = str(time) + " = " value_key = str(time) + " = " for key, value in zip( qsrlib_response_message.qsrs.trace[time].qsrs.keys(), qsrlib_response_message.qsrs.trace[time].qsrs.values()): object_key += str(key) + ';' argd_relation_msg.objects = object_key value_key += str(value.qsr) argd_relation_msg.relationship = value_key self.relationship_pub.publish(argd_relation_msg) print(argd_relation_msg)
def qsr_relation_between(obj1_name, obj2_name, obj1, obj2): global frame qsrlib = QSRlib() options = sorted(qsrlib.qsrs_registry.keys()) if init_qsr.qsr not in options: raise ValueError("qsr not found, keywords: %s" % options) world = World_Trace() object_types = {obj1_name: obj1_name, obj2_name: obj2_name} dynamic_args = { "qstag": { "object_types": object_types, "params": { "min_rows": 1, "max_rows": 1, "max_eps": 3 } }, "tpcc": { "qsrs_for": [(obj1_name, obj2_name)] }, "rcc2": { "qsrs_for": [(obj1_name, obj2_name)] }, "rcc4": { "qsrs_for": [(obj1_name, obj2_name)] }, "rcc8": { "qsrs_for": [(obj1_name, obj2_name)] } } o1 = [Object_State(name=obj1_name, timestamp=frame, x=obj1[0], y=obj1[1], \ xsize=obj1[2], ysize=obj1[3])] o2 = [Object_State(name=obj2_name, timestamp=frame, x=obj2[0], y=obj2[1], \ xsize=obj2[2], ysize=obj2[3])] world.add_object_state_series(o1) world.add_object_state_series(o2) qsrlib_request_message = QSRlib_Request_Message(which_qsr=init_qsr.qsr, \ input_data=world, dynamic_args=dynamic_args) qsrlib_response_message = qsrlib.request_qsrs( req_msg=qsrlib_request_message) pretty_print_world_qsr_trace(init_qsr.qsr, qsrlib_response_message) #, vis = True) qsr_value = find_qsr_value(init_qsr.qsr, qsrlib_response_message) return qsr_value
def convert_skeleton_to_world(self, data, use_every=1): world = World_Trace() joint_states = { 'head': [], 'neck': [], 'torso': [], 'left_shoulder': [], 'left_elbow': [], 'left_hand': [], 'left_hip': [], 'left_knee': [], 'left_foot': [], 'right_shoulder': [], 'right_elbow': [], 'right_hand': [], 'right_hip': [], 'right_knee': [], 'right_foot': [] } for tp, msg in enumerate(data): for i in msg.joints: o = Object_State(name=i.name, timestamp=tp, x=i.pose.position.x,\ y = i.pose.position.y, z=i.pose.position.z, xsize=0.1, ysize=0.1, zsize=0.1) joint_states[i.name].append(o) for joint_data in joint_states.values(): world.add_object_state_series(joint_data) return world
def ppl_cb(self, msg): self.__buffer["human"].append(msg.poses[0]) self.__buffer["robot"].append(self.robot_pose) # QTC needs at least two instances in time to work ob = [] if len(self.__buffer["human"]) > 1: # Creating the world trace for both agents over all timestamps world = World_Trace() for idx, (human, robot) in enumerate( zip(self.__buffer["human"], self.__buffer["robot"])): ob.append( Object_State(name="human", timestamp=idx, x=human.position.x, y=human.position.y)) ob.append( Object_State(name="robot", timestamp=idx, x=robot.position.x, y=robot.position.y)) world.add_object_state_series(ob) # Creating the qsr_lib request message qrmsg = QSRlib_Request_Message(which_qsr="qtccs", input_data=world, dynamic_args=self.__parameters) cln = QSRlib_ROS_Client() req = cln.make_ros_request_message(qrmsg) res = cln.request_qsrs(req) out = pickle.loads(res.data) # Printing the result, publishing might be more useful though ;) qsr_array = [] for t in out.qsrs.get_sorted_timestamps(): for k, v in out.qsrs.trace[t].qsrs.items(): qsr_array.append(v.qsr.values()[0]) #test self.send_dict(qsr_array) #print out rospy.sleep(0.3) # Worst smoothing ever, I'm sure you can do better
def _convert_to_world(self, data_dict, quantisation_factor=0, validate=True, no_collapse=False, distance_threshold=np.Inf): world = World_Trace() agent1 = data_dict["agent1"] agent2 = data_dict["agent2"] name1 = agent1["name"] name2 = agent2["name"] x1 = np.array(agent1["x"], dtype=float) y1 = np.array(agent1["y"], dtype=float) x2 = np.array(agent2["x"], dtype=float) y2 = np.array(agent2["y"], dtype=float) ob = [] for idx, (e_x1, e_y1, e_x2, e_y2) in enumerate(zip(x1, y1, x2, y2)): ob.append( Object_State(name=name1, timestamp=idx, x=e_x1, y=e_y1, quantisation_factor=quantisation_factor, validate=validate, no_collapse=no_collapse, distance_threshold=distance_threshold)) ob.append( Object_State(name=name2, timestamp=idx, x=e_x2, y=e_y2, quantisation_factor=quantisation_factor, validate=validate, no_collapse=no_collapse, distance_threshold=distance_threshold)) world.add_object_state_series(ob) return world
def _convert_to_world(self, data_dict): world = World_Trace() agent1 = data_dict["agent1"] agent2 = data_dict["agent2"] name1 = agent1["name"] name2 = agent2["name"] x1 = np.array(agent1["x"], dtype=float) y1 = np.array(agent1["y"], dtype=float) x2 = np.array(agent2["x"], dtype=float) y2 = np.array(agent2["y"], dtype=float) ob = [] for idx, (e_x1, e_y1, e_x2, e_y2) in enumerate(zip(x1, y1, x2, y2)): ob.append(Object_State(name=name1, timestamp=idx, x=e_x1, y=e_y1)) ob.append(Object_State(name=name2, timestamp=idx, x=e_x2, y=e_y2)) world.add_object_state_series(ob) return world
def relations_callback(self,extracted_pose1,extracted_pose2): qtc_relation_msg = QsrMsg() print("extracted_pose1",extracted_pose1.pose_vec.position) print("marker1", extracted_pose1.marker_id) print('==========================================') print("extracted_pose2",extracted_pose2.pose_vec.position) print("marker2", extracted_pose2.marker_id) print("##########################################") o1 = [Object_State(name="object_1", timestamp=self.prev_timestamp, x=self.prev_pose_x1, y=self.prev_pose_y1), Object_State(name="object_1", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose1.pose_vec.position.x, y=extracted_pose1.pose_vec.position.z)] o2 = [Object_State(name="robot", timestamp=self.prev_timestamp, x=self.prev_robot_pose_x, y=self.prev_robot_pose_y), Object_State(name="robot", timestamp=extracted_pose1.header.stamp.secs, x=0, y=0)] o3 = [Object_State(name="object_2", timestamp=self.prev_timestamp, x=self.prev_pose_x2, y=self.prev_pose_y2), Object_State(name="object_2", timestamp=extracted_pose1.header.stamp.secs, x=extracted_pose2.pose_vec.position.x, y=extracted_pose2.pose_vec.position.z)] self.world.add_object_state_series(o1) self.world.add_object_state_series(o2) self.world.add_object_state_series(o3) self.prev_timestamp = extracted_pose1.header.stamp.secs self.prev_pose_x1 = extracted_pose1.pose_vec.position.x self.prev_pose_y1 = extracted_pose1.pose_vec.position.z self.prev_pose_x2 = extracted_pose2.pose_vec.position.x self.prev_pose_y2 = extracted_pose2.pose_vec.position.z self.prev_robot_pose_x = 0 self.prev_robot_pose_y = 0 qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.calculi, input_data=self.world, dynamic_args=self.dynamic_args) req = self.cln.make_ros_request_message(qsrlib_request_message) res = self.cln.request_qsrs(req) qsrlib_response_message = pickle.loads(res.data) qtc_relation_msg.selected_qsr = str(self.calculi) qtc_relation_msg.marker_id1 = extracted_pose1.marker_id qtc_relation_msg.marker_id2 = extracted_pose2.marker_id for time in qsrlib_response_message.qsrs.get_sorted_timestamps(): qtc_relation_msg.message_time = extracted_pose1.header.stamp.secs object_key = str(extracted_pose1.header.stamp.secs) + " = " value_key = str(extracted_pose1.header.stamp.secs) + " = " for key, value in zip(qsrlib_response_message.qsrs.trace[time].qsrs.keys(), qsrlib_response_message.qsrs.trace[time].qsrs.values()): object_key += str(key) + ';' qtc_relation_msg.objects = object_key value_key += str(value.qsr) qtc_relation_msg.relationship = value_key self.relationship_pub.publish(qtc_relation_msg) print(qtc_relation_msg)
def get_world_frame_trace(self, world_objects): """Accepts a dictionary of world (soma) objects. Adds the position of the object at each timepoint into the World Trace Note 'frame' is the actual detection file number. 't' is a timestamp starting from 0. 't' is used in the World Trace.""" ob_states = {} world = World_Trace() # print ">>", self.map_frame_data.keys() for t, frame in enumerate(self.sorted_timestamps): #Joints: for joint_id, (x, y, z) in self.map_frame_data[frame].items(): if joint_id not in ob_states.keys(): ob_states[joint_id] = [ Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z) ] else: ob_states[joint_id].append( Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)) # SOMA objects for object, (x, y, z) in world_objects.items(): if object not in ob_states.keys(): ob_states[object] = [ Object_State(name=str(object), timestamp=t, x=x, y=y, z=z) ] else: ob_states[object].append( Object_State(name=str(object), timestamp=t, x=x, y=y, z=z)) # Robot's position (x, y, z) = self.robot_data[frame][0] if 'robot' not in ob_states.keys(): ob_states['robot'] = [ Object_State(name='robot', timestamp=t, x=x, y=y, z=z) ] else: ob_states['robot'].append( Object_State(name='robot', timestamp=t, x=x, y=y, z=z)) for object_state in ob_states.values(): world.add_object_state_series(object_state) self.map_world = world
def get_world_frame_trace(self, world_objects): """Accepts a dictionary of world (soma) objects. Adds the position of the object at each timepoint into the World Trace""" ob_states = {} world = World_Trace() for t in self.sorted_timestamps: #Joints: for joint_id, (x, y, z) in self.map_frame_data[t].items(): if joint_id not in ob_states.keys(): ob_states[joint_id] = [ Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z) ] else: ob_states[joint_id].append( Object_State(name=joint_id, timestamp=t, x=x, y=y, z=z)) # SOMA objects for object, (x, y, z) in world_objects.items(): if object not in ob_states.keys(): ob_states[object] = [ Object_State(name=str(object), timestamp=t, x=x, y=y, z=z) ] else: ob_states[object].append( Object_State(name=str(object), timestamp=t, x=x, y=y, z=z)) # Robot's position (x, y, z) = self.robot_data[t][0] if 'robot' not in ob_states.keys(): ob_states['robot'] = [ Object_State(name='robot', timestamp=t, x=x, y=y, z=z) ] else: ob_states['robot'].append( Object_State(name='robot', timestamp=t, x=x, y=y, z=z)) for object_state in ob_states.values(): world.add_object_state_series(object_state) self.map_world = world
def add_qsr(state, object_name, box): global frame x, y, xs, ys = box[0], box[1], box[2], box[3] state.append(Object_State(name=object_name, timestamp=frame, x=x, y=y, \ xsize=xs, ysize=ys)) return state
} beta_units = np.load(args.units) beta = np.load(args.beta) scores = np.load(args.scores) timestamp = np.load(args.timestamp) o1 = [] o2 = [] o3 = [] for ii, unit in tqdm(enumerate(beta_units)): o1.append( Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=unit, object_type="Unit")) for ii, beta in tqdm(enumerate(beta)): o2.append( Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=beta, object_type="Beta")) for ii, score in tqdm(enumerate(scores)): o3.append( Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=score,
} cauchy_units = np.load(args.units) cauchy = np.load(args.cauchy) scores = np.load(args.scores) timestamp = np.load(args.timestamp) o1 = [] o2 = [] o3 = [] for ii, unit in tqdm(enumerate(cauchy_units)): o1.append( Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=unit, object_type="Unit")) for ii, cauc in tqdm(enumerate(cauchy)): o2.append( Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=cauc, object_type="Cauchy")) for ii, score in tqdm(enumerate(scores)): o3.append( Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=score,
def __compute_qsr(self, bb1, bb2): if not self.qsr: return "" ax, ay, bx, by = bb1 cx, cy, dx, dy = bb2 qsrlib = QSRlib() world = World_Trace() if self.args.distribution[0] == "weibull": weibull_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/weibull_units.npy") weibull = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/weibull.npy") scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy") timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy") o1 = [] o2 = [] o3 = [] for ii,unit in tqdm(enumerate(weibull_units)): o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit")) for ii,weib in tqdm(enumerate(weibull)): o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(weib*100), object_type="Weibull")) for ii,score in tqdm(enumerate(scores)): o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score")) elif self.args.distribution[0] == "beta": beta_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta_units.npy") beta = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta.npy") scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy") timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy") o1 = [] o2 = [] o3 = [] for ii,unit in tqdm(enumerate(beta_units)): o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit")) for ii,beta in tqdm(enumerate(beta)): o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(beta*100), object_type="Beta")) for ii,score in tqdm(enumerate(scores)): o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score")) elif self.args.distribution[0] == "cauchy": cauchy_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/cauchy_units.npy") cauchy = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/cauchy.npy") scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy") timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy") o1 = [] o2 = [] o3 = [] for ii,unit in tqdm(enumerate(cauchy_units)): o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit")) for ii,cauc in tqdm(enumerate(cauchy)): o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(cauc*100), object_type="Cauchy")) for ii,score in tqdm(enumerate(scores)): o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score")) elif self.args.distribution[0] == "rayleigh": rayleigh_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/rayleigh_units.npy") rayleigh = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/beta.npy") scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy") timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy") o1 = [] o2 = [] o3 = [] for ii,unit in tqdm(enumerate(rayleigh_units)): o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit")) for ii,rayl in tqdm(enumerate(rayleigh)): o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(rayl*100), object_type="Rayleigh")) for ii,score in tqdm(enumerate(scores)): o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score")) elif self.args.distribution[0] == "gamma": gamma_units = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/gamma_units.npy") gamma = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/gamma.npy") scores = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/scaled_scores.npy") timestamp = np.load("/home/aswin/Documents/Courses/Udacity/Intel-Edge/Work/EdgeApp/PGCR-Results-Analysis/timestamp.npy") o1 = [] o2 = [] o3 = [] for ii,unit in tqdm(enumerate(gamma_units)): o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=int(unit*100), object_type="Unit")) for ii,gam in tqdm(enumerate(gamma)): o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=int(gam*100), object_type="Gamma")) for ii,score in tqdm(enumerate(scores)): o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=int(score*100), object_type="Score")) world.add_object_state_series(o1) world.add_object_state_series(o2) world.add_object_state_series(o3) # world.add_object_state_series([Object_State(name="red", timestamp=0, x=((ax+bx)/2.0), y=((ay+by)/2.0), xsize=abs(bx-ax), ysize=abs(by-ay)), # Object_State(name="yellow", timestamp=0, x=((cx+dx)/2.0), y=((cy+dy)/2.0), xsize=abs(dx-cx), ysize=abs(dy-cy))]) dynamic_args = {"argd": {"qsr_relations_and_values": self.distance}} qsrlib_request_message = QSRlib_Request_Message(which_qsr=self.qsr, input_data=world, dynamic_args=dynamic_args) qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message) for t in qsrlib_response_message.qsrs.get_sorted_timestamps(): foo = "" for k, v in zip(qsrlib_response_message.qsrs.trace[t].qsrs.keys(), qsrlib_response_message.qsrs.trace[t].qsrs.values()): foo += str(k) + ":" + str(v.qsr) + "; \n" return foo
}, "argd": { "qsr_relations_and_values": args.distance_threshold, "qsrs_for": argd_qsrs_for }, "mos": { "qsrs_for": mos_qsrs_for }, "qstag": { "object_types": object_types } } o1 = [ Object_State( name="o1", timestamp=0, x=2., y=2., object_type="Person" ), #accessed first using try: kwargs["object_type"] except: Object_State(name="o1", timestamp=1, x=1., y=1., object_type="Person") ] #Object_State(name="o1", timestamp=2, x=2., y=2., object_type="Human"), #Object_State(name="o1", timestamp=3, x=4., y=1., object_type="Human"), #Object_State(name="o1", timestamp=4, x=4., y=1., object_type="Human"), #Object_State(name="o1", timestamp=5, x=4., y=2., object_type="Human")] o2 = [ Object_State(name="o2", timestamp=0, x=1., y=1., object_type="Chair"), Object_State(name="o2", timestamp=1, x=2., y=2., object_type="Chair") ] #Object_State(name="o2", timestamp=2, x=1., y=5., object_type="Chair"), #Object_State(name="o2", timestamp=3, x=1., y=5., object_type="Chair"), #Object_State(name="o2", timestamp=4, x=2., y=5., object_type="Chair"),
"tpcc" : {"qsrs_for": tpcc_qsrs_for} #"filters": {"median_filter" : {"window": 2}} } gamma_units = np.load(args.units) gamma = np.load(args.gamma) scores = np.load(args.scores) timestamp = np.load(args.timestamp) o1 = [] o2 = [] o3 = [] for ii,unit in tqdm(enumerate(gamma_units)): o1.append(Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=unit, object_type="Unit")) for ii,gam in tqdm(enumerate(gamma)): o2.append(Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=gam, object_type="Gamma")) for ii,score in tqdm(enumerate(scores)): o3.append(Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=score, object_type="Score")) world.add_object_state_series(o1) world.add_object_state_series(o2) world.add_object_state_series(o3) qsrlib_request_message = QSRlib_Request_Message(which_qsr=which_qsr, input_data=world, dynamic_args=dynamic_args) print(which_qsr) print(dynamic_args["tpcc"]) t1 = time()
def get_world_frame_trace(self, world_objects): """Accepts a dictionary of world (soma) objects. Adds the position of the object at each timepoint into the World Trace""" self.subj_world_trace = {} for subj in self.skeleton_map: ob_states = {} world = World_Trace() map_frame_data = self.skeleton_map[subj] for joint_id in map_frame_data.keys(): #Joints: for t in xrange(self.frames): x = map_frame_data[joint_id][t][0] y = map_frame_data[joint_id][t][1] z = map_frame_data[joint_id][t][2] if joint_id not in ob_states.keys(): ob_states[joint_id] = [ Object_State(name=joint_id, timestamp=t + 1, x=x, y=y, z=z) ] else: ob_states[joint_id].append( Object_State(name=joint_id, timestamp=t + 1, x=x, y=y, z=z)) # SOMA objects for t in xrange(self.frames): for object, (x, y, z) in world_objects.items(): if object not in ob_states.keys(): ob_states[object] = [ Object_State(name=str(object), timestamp=t + 1, x=x, y=y, z=z) ] else: ob_states[object].append( Object_State(name=str(object), timestamp=t + 1, x=x, y=y, z=z)) # # Robot's position # (x,y,z) = self.robot_data[t][0] # if 'robot' not in ob_states.keys(): # ob_states['robot'] = [Object_State(name='robot', timestamp=t, x=x, y=y, z=z)] # else: # ob_states['robot'].append(Object_State(name='robot', timestamp=t, x=x, y=y, z=z)) for obj, object_state in ob_states.items(): world.add_object_state_series(object_state) # get world trace for each person region = self.soma_roi_config[self.waypoint] self.subj_world_trace[subj] = self.get_object_frame_qsrs( world, self.objects[region])
for bag_path in lab_bags: bag = rosbag.Bag(bag_path) r_xs = [] r_ys = [] r_state_seq = [] for topic, msg, t in bag.read_messages(topics=['/robot5/control/odom']): t = t.to_sec() x = msg.pose.pose.position.x y = msg.pose.pose.position.y r_xs.append(x) r_ys.append(y) r_state_seq.append(Object_State(name="robot", timestamp=t, x=x, y=y)) r_state_seqs.append(r_state_seq) h_xs = [] h_ys = [] h_state_seq = [] for topic, msg, t in bag.read_messages( topics=['/robot5/people_tracker_filtered/positions']): t = t.to_sec() try: x = msg.poses[0].position.x y = msg.poses[0].position.y h_xs.append(x) h_ys.append(y)
raise ValueError("qsr not found, keywords: %s" % options) world = World_Trace() dynamic_args = {} if which_qsr in ["rcc2", "rcc3", "ra"]: dynamic_args = { which_qsr: { "quantisation_factor": args.quantisation_factor } } o1 = [ Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.), Object_State(name="o1", timestamp=1, x=1., y=2., xsize=5., ysize=8.), Object_State(name="o1", timestamp=2, x=1., y=3., xsize=5., ysize=8.) ]
def apply_median_filter(self, window_size=11, vis=False): """Once obtained the joint x,y,z coords. Apply a median filter over a temporal window to smooth the joint positions. Whilst doing this, create a world Trace object""" fx = 525.0 fy = 525.0 cx = 319.5 cy = 239.5 data, f_data, ob_states = {}, {}, {} self.camera_world = World_Trace() self.filtered_skeleton_data = {} for t in self.sorted_timestamps: self.filtered_skeleton_data[t] = { } # initialise with all timepoints for joint_id, (x, y, z) in self.skeleton_data[t].items(): try: data[joint_id]["x"].append(x) data[joint_id]["y"].append(y) data[joint_id]["z"].append(z) except KeyError: data[joint_id] = {"x": [x], "y": [y], "z": [z]} for joint_id, joint_dic in data.items(): f_data[joint_id] = {} for dim, values in joint_dic.items(): filtered_values = sp.signal.medfilt(values, window_size) # filter f_data[joint_id][dim] = filtered_values if dim is "z" and 0 in filtered_values: print ">> Z should never be 0. (distance to camera)." return False #vis = True if vis and "hand" in joint_id: print joint_id title1 = 'input %s position: %s' % (joint_id, dim) title2 = 'filtered %s position: %s' % (joint_id, dim) #plot_the_results(values, filtered_values, title1, title2) # Create a QSRLib format list of Object States (for each joint id) for cnt, t in enumerate(self.sorted_timestamps): x = f_data[joint_id]["x"][cnt] y = f_data[joint_id]["y"][cnt] z = f_data[joint_id]["z"][cnt] # add the x2d and y2d (using filtered x,y,z data) # print self.uuid, t, joint_id, (x,y,z) try: x2d = int(x * fx / z * 1 + cx) y2d = int(y * fy / z * -1 + cy) except ValueError: print "ValueError for %s at frame: %s. t:%s" % (self.uuid, cnt, t) x2d = 0 y2d = 0 self.filtered_skeleton_data[t][joint_id] = ( x, y, z, x2d, y2d) # Kept for Legacy try: ob_states[joint_id].append( Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z)) except KeyError: ob_states[joint_id] = [ Object_State(name=joint_id, timestamp=cnt, x=x, y=y, z=z) ] # #Add all the joint IDs into the World Trace for joint_id, obj in ob_states.items(): self.camera_world.add_object_state_series(obj) return True
options = sorted(qsrlib.qsrs_registry.keys()) #Use print statement to see what are the possible calculi that can be used to obtain spatial relationships from the QSRlib # print ("These are the options available for matching", options) #create a list of the calculi obtained from the user's input this_qsr = args.qsr #==============================================================================================================# #=================================Mock up data for testing=====================================================# # creating input data, he basic data structure used in QSRlib is the World_Trace object world = World_Trace() #creating mulitple spatial entities with their respective poses, ids and sizes for mulitple timestamp everything is in terms of pixels o1 = [ Object_State(name="o1", timestamp=0, x=226, y=287., xsize=5., ysize=8.), Object_State(name="o1", timestamp=1, x=308., y=289., xsize=5., ysize=8.), Object_State(name="o1", timestamp=2, x=330., y=324., xsize=5., ysize=8.), Object_State(name="o1", timestamp=3, x=387., y=349., xsize=5., ysize=8.) ] #creating a second spatial entity with different poses o2 = [ Object_State(name="o2", timestamp=0, x=500, y=330, xsize=5., ysize=8.), Object_State(name="o2", timestamp=1, x=1026., y=262., xsize=5., ysize=8.), Object_State(name="o2", timestamp=2, x=1144., y=247., xsize=5., ysize=8.), Object_State(name="o2", timestamp=3, x=1268., y=262., xsize=5., ysize=8.) ] o3 = [ Object_State(name="o3", timestamp=0, x=640., y=360., xsize=10, ysize=10),
} weibull_units = np.load(args.units) weibull = np.load(args.weibull) scores = np.load(args.scores) timestamp = np.load(args.timestamp) o1 = [] o2 = [] o3 = [] for ii, unit in tqdm(enumerate(weibull_units)): o1.append( Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=unit, object_type="Unit")) for ii, weib in tqdm(enumerate(weibull)): o2.append( Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=weib, object_type="Weibull")) for ii, score in tqdm(enumerate(scores)): o3.append( Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=score,
} rayleigh_units = np.load(args.units) rayleigh = np.load(args.rayleigh) scores = np.load(args.scores) timestamp = np.load(args.timestamp) o1 = [] o2 = [] o3 = [] for ii, unit in tqdm(enumerate(rayleigh_units)): o1.append( Object_State(name="o1", timestamp=timestamp[ii], x=ii, y=unit, object_type="Unit")) for ii, rayl in tqdm(enumerate(rayleigh)): o2.append( Object_State(name="o2", timestamp=timestamp[ii], x=ii, y=rayl, object_type="Rayleigh")) for ii, score in tqdm(enumerate(scores)): o3.append( Object_State(name="o3", timestamp=timestamp[ii], x=ii, y=score,
# parse command line arguments options = sorted(qsrlib.qsrs_registry.keys()) print(options) parser = argparse.ArgumentParser() parser.add_argument("qsr", help="choose qsr: %s" % options, type=str) args = parser.parse_args() if args.qsr in options: which_qsr = args.qsr else: raise ValueError("qsr not found, keywords: %s" % options) # **************************************************************************************************** # make some input data world = World_Trace() o1 = [ Object_State(name="o1", timestamp=0, x=1., y=10., xsize=5., ysize=8.), Object_State(name="o1", timestamp=1, x=1., y=10., xsize=5., ysize=8.), Object_State(name="o1", timestamp=2, x=3., y=1., xsize=5., ysize=8.), Object_State(name="o1", timestamp=3, x=6., y=1., xsize=5., ysize=8.), Object_State(name="o1", timestamp=4, x=6., y=1., xsize=5., ysize=8.) ] o2 = [ Object_State(name="o2", timestamp=0, x=1., y=1., xsize=5., ysize=8.), Object_State(name="o2", timestamp=1, x=2., y=1., xsize=5., ysize=8.), Object_State(name="o2", timestamp=2, x=1., y=1., xsize=5., ysize=8.), Object_State(name="o2", timestamp=3, x=1., y=1., xsize=5., ysize=8.), Object_State(name="o2", timestamp=4, x=1., y=2., xsize=5., ysize=8.) ] o3 = [ Object_State(name="o3", timestamp=0, x=5., y=2., xsize=5.2, ysize=8.5), Object_State(name="o3", timestamp=1, x=6., y=4., xsize=5.2, ysize=8.5),
print("-t:", t) for oname, o in world_trace.trace[t].objects.items(): print("%s\t%f\t%f\t%f\t%f\t%f\t%f" % (oname, o.x, o.y, o.z, o.xsize, o.ysize, o.xsize)) def print_world_state(world_state): for oname, o in world_state.objects.items(): print("%s\t%f\t%f\t%f\t%f\t%f\t%f" % (oname, o.x, o.y, o.z, o.xsize, o.ysize, o.xsize)) if __name__ == "__main__": world = World_Trace() o1 = [ Object_State(name="o1", timestamp=0, x=1., y=1., xsize=5., ysize=8.), Object_State(name="o1", timestamp=1, x=1., y=2., xsize=5., ysize=8.), Object_State(name="o1", timestamp=2, x=1., y=3., xsize=5., ysize=8.), Object_State(name="o1", timestamp=3, x=1., y=4., xsize=5., ysize=8.), Object_State(name="o1", timestamp=4, x=1., y=5., xsize=5., ysize=8.) ] o2 = [ Object_State(name="o2", timestamp=0, x=11., y=1., xsize=5., ysize=8.), Object_State(name="o2", timestamp=1, x=11., y=2., xsize=5., ysize=8.), Object_State(name="o2", timestamp=2, x=11., y=3., xsize=5., ysize=8.), Object_State(name="o2", timestamp=3, x=11., y=4., xsize=5., ysize=8.), Object_State(name="o2", timestamp=4, x=11., y=5., xsize=5., ysize=8.) ] world.add_object_state_series(o1) world.add_object_state_series(o2)
#creating a empty list to which we will append the data obj = [] with open(args.input) as csvfile: # creating a csv object to parse the info from the csv file file = csv.DictReader(csvfile) print("Reading file '%s':" % args.input) for idx,row in enumerate(file): # using the column headers to determine the object names and their pose and timestamp, the inbuilt csv index is used as a timestamp obj.append(Object_State( name=row['agent1'], timestamp=idx, x=float(row['x1']), y=float(row['y1']) )) obj.append(Object_State( name=row['agent2'], timestamp=idx, x=float(row['x2']), y=float(row['y2']) )) #adding the objects(spatial entities) to the created World_Trace object world.add_object_state_series(obj) else: #creating mulitple spatial entities with their respective poses, ids and sizes for mulitple timestamp everything is in terms of pixels