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
