def _request_qtc(self, qsr, world, parameters):
qrmsg = QSRlib_Request_Message(which_qsr=qsr,
input_data=world,
dynamic_args=parameters)
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qrmsg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
qtc = []
dis = []
for t in out.qsrs.get_sorted_timestamps():
for k, v in out.qsrs.trace[t].qsrs.items():
# print v.qsr.items()
# if len(v.qsr.items()) < 2:
# continue # Hacky but we only want dist when qtc is there too.
for l, w in v.qsr.items():
if l.startswith("qtc"):
# q = self._to_np_array(w)
# if l.startswith("qtcbcs"):
# q = q if len(q) == 4 else np.append(q, [np.nan, np.nan])
# qtc = np.array([q]) if not qtc.size else np.append(qtc, [q], axis=0)
qtc.append(w)
elif l == "argprobd":
dis.append(w)
return qtc, dis
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 _request_qtc(self, qsr, world, include_missing_data=True):
"""reads all .qtc files from a given directory and resturns them as numpy arrays"""
qrmsg = QSRlib_Request_Message(
which_qsr=qsr,
input_data=world,
include_missing_data=include_missing_data)
cln = QSRlib_ROS_Client()
req = cln.make_ros_request_message(qrmsg)
res = cln.request_qsrs(req)
out = pickle.loads(res.data)
rospy.logdebug("Request was made at " +
str(out.timestamp_request_made) + " and received at " +
str(out.timestamp_request_received) +
" and computed at " + str(out.timestamp_qsrs_computed))
ret = np.array([])
for t in out.qsrs.get_sorted_timestamps():
foo = str(t) + ": "
for k, v in zip(out.qsrs.trace[t].qsrs.keys(),
out.qsrs.trace[t].qsrs.values()):
foo += str(k) + ":" + str(v.qsr) + "; "
if qsr == self.qtc_types["qtcbc"]:
q = v.qsr if len(v.qsr) == 4 else np.append(
v.qsr, [np.nan, np.nan])
ret = np.array([q]) if not ret.size else np.append(
ret, [q], axis=0)
else:
ret = np.array([v.qsr]) if not ret.size else np.append(
ret, [v.qsr], axis=0)
rospy.logdebug(foo)
return ret
def get_object_frame_qsrs(self, world_trace, objects):
joint_types = {
'left_hand': 'hand',
'right_hand': 'hand',
'head-torso': 'tpcc-plane'
}
joint_types_plus_objects = joint_types.copy()
for object in objects:
generic_object = "_".join(object.split("_")[:-1])
joint_types_plus_objects[object] = generic_object
#print joint_types_plus_objects
"""create QSRs between the person's joints and the soma objects in map frame"""
qsrs_for = []
for ob in objects:
qsrs_for.append((str(ob), 'left_hand'))
qsrs_for.append((str(ob), 'right_hand'))
#qsrs_for.append((str(ob), 'torso'))
dynamic_args = {}
# dynamic_args['argd'] = {"qsrs_for": qsrs_for, "qsr_relations_and_values": {'Touch': 0.25, 'Near': 0.5, 'Ignore': 10}}
# dynamic_args['qtcbs'] = {"qsrs_for": qsrs_for, "quantisation_factor": 0.05, "validate": False, "no_collapse": True} # Quant factor is effected by filters to frame rate
# dynamic_args["qstag"] = {"object_types": joint_types_plus_objects, "params": {"min_rows": 1, "max_rows": 1, "max_eps": 2}}
dynamic_args['argd'] = {
"qsrs_for": qsrs_for,
"qsr_relations_and_values": {
'Touch': 0.5,
'Near': 0.75,
'Medium': 1.5,
'Ignore': 10
}
}
# dynamic_args['argd'] = {"qsrs_for": qsrs_for, "qsr_relations_and_values": {'Touch': 0.2, 'Ignore': 10}}
dynamic_args['qtcbs'] = {
"qsrs_for": qsrs_for,
"quantisation_factor": 0.01,
"validate": False,
"no_collapse": True
} # Quant factor is effected by filters to frame rate
dynamic_args["qstag"] = {
"object_types": joint_types_plus_objects,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 2
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr=["argd", "qtcbs"],
input_data=world_trace,
dynamic_args=dynamic_args)
#req = QSRlib_Request_Message(which_qsr="argd", input_data=world_trace, dynamic_args=dynamic_args)
ret = qsrlib.request_qsrs(req_msg=req)
# for ep in ret.qstag.episodes:
# print ep
#
return ret
def get_map_frame_qsrs(file, world_trace, dynamic_args):
"""create QSRs between the person and the robot in map frame"""
qsrs_for = [('robot', 'torso')]
dynamic_args['qtcbs'] = {
"qsrs_for": qsrs_for,
"quantisation_factor": 0.0,
"validate": False,
"no_collapse": True
}
dynamic_args["qstag"] = {
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 3
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr="qtcbs",
input_data=world_trace,
dynamic_args=dynamic_args)
qsr_map_frame = qsrlib.request_qsrs(req_msg=req)
print " ", file, "episodes = "
for i in qsr_map_frame.qstag.episodes:
print i
return qsr_map_frame
def _create_qsr(self, qsr, dynamic_args=None):
dynamic_args = self.dynamic_args if not dynamic_args else dynamic_args
qsrlib_request_message = QSRlib_Request_Message(
which_qsr=qsr,
input_data=self.world,
dynamic_args=dynamic_args
)
return self._request(qsrlib_request_message)
def custom(self, world_name, gt_filename, dynamic_args):
expected = unittest_read_qsrs_as_one_long_list(
find_resource(PKG, gt_filename)[0])
actual = unittest_get_multiple_qsrs_as_one_long_list(
self._qsrlib.request_qsrs(
QSRlib_Request_Message(self.__which_qsr,
self._worlds[world_name],
dynamic_args)).qsrs, self.__which_qsr)
return expected, actual
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 qsrs_for_qsr_namespace(self, world_name, gt_filename):
expected = unittest_read_qsrs_as_one_long_list(
find_resource(PKG, gt_filename)[0])
req_msg = QSRlib_Request_Message(
self._unique_id, self._worlds[world_name],
{self._unique_id: {
"qsrs_for": ["o1"]
}})
actual = unittest_get_qsrs_as_one_long_list(
self._qsrlib.request_qsrs(req_msg).qsrs)
return expected, actual
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 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 get_object_frame_qsrs(file, world_trace, objects, joint_types,
dynamic_args):
"""create QSRs between the person's joints and the soma objects in map frame"""
qsrs_for = []
for ob in objects:
qsrs_for.append((str(ob), 'left_hand'))
#qsrs_for.append((str(ob), 'right_hand'))
#qsrs_for.append((str(ob), 'torso'))
dynamic_args['argd'] = {
"qsrs_for": qsrs_for,
"qsr_relations_and_values": {
'Touch': 0.5,
'Near': 0.75,
'Medium': 1.5,
'Ignore': 10
}
}
# dynamic_args['argd'] = {"qsrs_for": qsrs_for, "qsr_relations_and_values": {'Touch': 0.2, 'Ignore': 10}}
dynamic_args['qtcbs'] = {
"qsrs_for": qsrs_for,
"quantisation_factor": 0.05,
"validate": False,
"no_collapse": True
} # Quant factor is effected by filters to frame rate
dynamic_args["qstag"] = {
"object_types": joint_types,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 2
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr=["argd", "qtcbs"],
input_data=world_trace,
dynamic_args=dynamic_args)
# req = QSRlib_Request_Message(which_qsr="argd", input_data=world_trace, dynamic_args=dynamic_args)
qsr_object_frame = qsrlib.request_qsrs(req_msg=req)
for ep in qsr_object_frame.qstag.episodes:
print ep
for cnt, h in zip(qsr_object_frame.qstag.graphlets.histogram,
qsr_object_frame.qstag.graphlets.code_book):
print "\n", cnt, h, qsr_object_frame.qstag.graphlets.graphlets[h]
return qsr_object_frame
def qsrs_for_qsr_namespace_over_global_namespace(self, world_name,
gt_filename):
expected = unittest_read_qsrs_as_one_long_list(
find_resource(PKG, gt_filename)[0])
req_msg = QSRlib_Request_Message(
self._unique_id, self._worlds[world_name], {
"for_all_qsrs": {
"qsrs_for": [("o3", "o2", "o1")]
},
self._unique_id: {
"qsrs_for": [("o1", "o2", "o3")]
}
})
actual = unittest_get_qsrs_as_one_long_list(
self._qsrlib.request_qsrs(req_msg).qsrs)
return actual, expected
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 _call_qsrLib(self, uuid, msg_data):
qsrlib_request_message = QSRlib_Request_Message(which_qsr=self._which_qsr, input_data=msg_data["world"], \
dynamic_args=self.dynamic_args)
req = self.cln.make_ros_request_message(qsrlib_request_message)
if self._logging:
print "logging the world trace"
msg = QSRlibMongo(uuid=uuid,
data=pickle.dumps(msg_data["world"]),
start_time=msg_data["start_time"],
end_time=msg_data["end_time"])
query = {"uuid": uuid}
self._store_client_world.update(message=msg,
message_query=query,
upsert=True)
res = self.cln.request_qsrs(req)
qsrlib_response_message = pickle.loads(res.data)
return qsrlib_response_message
def qsr_message(state_series, objects):
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()
# Create dynamic arguments for every qsr type
dynamic_args = dynamic_qsr_arguments(objects)
# Add all object states in the World.
for o in state_series:
world.add_object_state_series(o)
# Create request message
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)
def get_joint_frame_qsrs(file, world_trace, joint_types, dynamic_args):
qsrs_for = [('head', 'torso',
ob) if ob not in ['head', 'torso'] and ob != 'head-torso' else
() for ob in joint_types.keys()]
dynamic_args['tpcc'] = {"qsrs_for": qsrs_for}
dynamic_args["qstag"] = {
"object_types": joint_types,
"params": {
"min_rows": 1,
"max_rows": 1,
"max_eps": 3
}
}
qsrlib = QSRlib()
req = QSRlib_Request_Message(which_qsr="tpcc",
input_data=world_trace,
dynamic_args=dynamic_args)
qsr_joints_frame = qsrlib.request_qsrs(req_msg=req)
# for i in qsr_joints_frame.qstag.episodes:
# print i
return qsr_joints_frame
def worker_qsrs(chunk):
(file_, path, config) = chunk
#print "\n", path, file_
e = utils.load_e(path, file_)
dynamic_args = {}
try:
dynamic_args['argd'] = config['argd_args']
dynamic_args['qtcbs'] = config['qtcbs_args']
dynamic_args["qstag"] = {"params": config['qstag_args']}
dynamic_args['filters'] = {
"median_filter": {
"window": config['qsr_mean_window']
}
} # This has been updated since ECAI paper.
except KeyError:
print "check argd, qtcbs, qstag parameters in config file"
joint_types = {
'head': 'head',
'torso': 'torso',
'left_hand': 'hand',
'right_hand': 'hand',
'left_knee': 'knee',
'right_knee': 'knee',
'left_shoulder': 'shoulder',
'right_shoulder': 'shoulder',
'head-torso': 'tpcc-plane'
}
object_types = joint_types.copy()
for ob, pos in e.objects.items():
try:
generic_object = "_".join(ob.split("_")[:-1])
object_types[ob] = generic_object
# print "object generic", generic_object
except:
print "didnt add:", object
dynamic_args["qstag"]["object_types"] = object_types
# for i,j in object_types.items():
# print ">", i,j
"""1. CREATE QSRs FOR joints & Object """
qsrlib = QSRlib()
qsrs_for = []
for ob, pos in e.objects.items():
qsrs_for.append((str(ob), 'left_hand'))
qsrs_for.append((str(ob), 'right_hand'))
qsrs_for.append((str(ob), 'torso'))
dynamic_args['argd']["qsrs_for"] = qsrs_for
dynamic_args['qtcbs']["qsrs_for"] = qsrs_for
# for (i,j) in qsrs_for:
# print ">", (i,j)
req = QSRlib_Request_Message(config['which_qsr'],
input_data=e.map_world,
dynamic_args=dynamic_args)
e.qsr_object_frame = qsrlib.request_qsrs(req_msg=req)
# print ">", e.qsr_object_frame.qstag.graphlets.histogram
# for i in e.qsr_object_frame.qstag.episodes:
# print i
# sys.exit(1)
"""2. CREATE QSRs for joints - TPCC"""
# print "TPCC: ",
# # e.qsr_joint_frame = get_joint_frame_qsrs(file, e.camera_world, joint_types, dynamic_args)
# qsrs_for = [('head', 'torso', ob) if ob not in ['head', 'torso'] and ob != 'head-torso' else () for ob in joint_types.keys()]
# dynamic_args['tpcc'] = {"qsrs_for": qsrs_for}
# dynamic_args["qstag"]["params"] = {"min_rows": 1, "max_rows": 2, "max_eps": 4}
# qsrlib = QSRlib()
# req = QSRlib_Request_Message(which_qsr="tpcc", input_data=e.camera_world, dynamic_args=dynamic_args)
# e.qsr_joints_frame = qsrlib.request_qsrs(req_msg=req)
# # pretty_print_world_qsr_trace("tpcc", e.qsr_joints_frame)
# # print e.qsr_joints_frame.qstag.graphlets.histogram
utils.save_event(e, "Learning/QSR_Worlds")
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,
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()
if args.ros:
try:
import rospy
from qsrlib_ros.qsrlib_ros_client import QSRlib_ROS_Client
except ImportError:
raise ImportError("ROS not found")
client_node = rospy.init_node("qsr_lib_ros_client_example")
cln = QSRlib_ROS_Client()
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)
#### WORLD_QSR_TRACE DBG
# which_qsr = "rcc2"
which_qsr = ["mos", "rcc2", "cardir"]
qsrlib = QSRlib()
qsrlib_request_message = QSRlib_Request_Message(which_qsr, world)
qsrlib_response_message = qsrlib.request_qsrs(qsrlib_request_message)
qsrs = qsrlib_response_message.qsrs
print(">> original")
pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message)
print()
# last_state = qsrs.get_last_state(return_by_reference=True)
# t = last_state.timestamp
# last_state.qsrs["o1,o2"].qsr["rcc2"] = "c"
# foo = str(t) + ": "
# for k, v in zip(last_state.qsrs.keys(), last_state.qsrs.values()):
# foo += str(k) + ":" + str(v.qsr) + "; "
# print(foo)
# qsrs_new = qsrs.get_for_objects(["o1,o2"])
# qsrs_for_qsr_namespace - cherry pick
# dynamic_args["rcc2"] = {"qsrs_for": [("o1", "o2")]}
# dynamic_args["qtcbs"]["qsrs_for"] = [("o1", "o2")]
# dynamic_args["mwe"] = {"qsrs_for": [("o1", "o2")]}
# dynamic_args["mos"] = {"qsrs_for": ["o1"]}
# dynamic_args["tpcc"] = {"qsrs_for": [("o1", "o2", "o3")]}
# qsrs_for_qsr_namespace_over_global - cherry pick
# dynamic_args["for_all_qsrs"] = {"qsrs_for": [("o3", "o2", "o1"), ("o2", "o1"), "o2"]}
# dynamic_args["rcc2"] = {"qsrs_for": [("o1", "o2")]}
# dynamic_args["qtcbs"]["qsrs_for"] = [("o1", "o2")]
# dynamic_args["mwe"] = {"qsrs_for": [("o1", "o2")]}
# dynamic_args["mos"] = {"qsrs_for": ["o1"]}
# dynamic_args["tpcc"] = {"qsrs_for": [("o1", "o2", "o3")]}
print("> Computing QSRs", which_qsr, dynamic_args)
qsrlib_request_message = QSRlib_Request_Message(which_qsr, world, dynamic_args)
# request your QSRs
qsrlib_response_message = qsrlib.request_qsrs(qsrlib_request_message)
# ****************************************************************************************************
# save
qsrs = qsrlib_response_message.qsrs
print(len(qsrs.trace))
t = qsrs.get_sorted_timestamps()[1]
print(qsrs.trace[t].qsrs)
if args.qsr != "multiple":
qsrs_list = unittest_get_qsrs_as_one_long_list(qsrs)
else:
qsrs_list = unittest_get_multiple_qsrs_as_one_long_list(qsrs, which_qsr)
print("> Saving to:", args.output)
unittest_write_qsrs_as_one_long_list(qsrs_list, args.output)
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
# In[83]:
world.add_object_state_series(h_state_seq)
world.add_object_state_series(r_state_seq)
# make a QSRlib request message
dynamic_args = {
"qtccs": {
"no_collapse": True,
"quantisation_factor": quantisation_factor,
"validate": False,
"qsrs_for": [("human", "robot")]
}
}
qsrlib_request_message = QSRlib_Request_Message('qtccs', world, dynamic_args)
# request your QSRs
qsrlib_response_message = qsrlib.request_qsrs(req_msg=qsrlib_request_message)
qsrlib_response_message
# In[9]:
bag_path
# In[11]:
for bag_no in range(len(lab_bags)):
matplotlib.rcParams['figure.figsize'] = (20.0, 10.0)
matplotlib.rcParams['font.size'] = 30
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),
Object_State(name="o3", timestamp=2, x=2., y=4., xsize=5.2, ysize=8.5),
Object_State(name="o3", timestamp=3, x=1., y=4., xsize=5.2, ysize=8.5),
Object_State(name="o3", timestamp=4, x=0., y=4., xsize=5.2, ysize=8.5)
]
world.add_object_state_series(o1)
world.add_object_state_series(o2)
world.add_object_state_series(o3)
# ****************************************************************************************************
dynammic_args = {
"qsr_relations_and_values": {
"Touch": 0.5,
"Near": 1,
"Far": 3
}
}
# make a QSRlib request message
qsrlib_request_message = QSRlib_Request_Message(
which_qsr, world, dynammic_args["qsr_relations_and_values"])
# request your QSRs
qsrlib_response_message = qsrlib.request_qsrs(
req_msg=qsrlib_request_message)
# ****************************************************************************************************
# print out your QSRs
pretty_print_world_qsr_trace(which_qsr, qsrlib_response_message)
def custom(self, world_name, gt_filename, dynamic_args):
expected = unittest_read_qsrs_as_one_long_list(find_resource(PKG, gt_filename)[0])
req_msg = QSRlib_Request_Message(self._unique_id, self._worlds[world_name], dynamic_args)
actual = unittest_get_qsrs_as_one_long_list(self._qsrlib.request_qsrs(req_msg).qsrs)
return expected, actual
def q_factor(self, world_name, gt_filename, q_factor):
expected = unittest_read_qsrs_as_one_long_list(find_resource(PKG, gt_filename)[0])
req_msg = QSRlib_Request_Message(self._unique_id, self._worlds[world_name],
{self._unique_id: {"quantisation_factor": q_factor}})
actual = unittest_get_qsrs_as_one_long_list(self._qsrlib.request_qsrs(req_msg).qsrs)
return expected, actual
#
# #creating a list to store the response from the QSRlib
qsrlib_response_message = list()
#iterating over the entire list to obtain all the individual calculi for which the spatial relationships will be calculated
for i in range(len(this_qsr)):
#performs a check to see if the input qsr's are present in the QSRlib library
if this_qsr[i] in options:
#printing them out in a readable format, just for clarity
print("These are the requested qsr's:%s" % (this_qsr[i]) +
", this is the qsr id:%i" % (i))
#creating a message to the QSRlib for calculating the relationships
# qsrlib_request_message.append(QSRlib_Request_Message(this_qsr[i], world))
qsrlib_request_message.append(
QSRlib_Request_Message(this_qsr[i], world))
#parsing the received arguments
# qsrlib_response_message.append(qsrlib.request_qsrs(req_msg=qsrlib_request_message[i]))
qsrlib_response_message.append(
qsrlib.request_qsrs(req_msg=qsrlib_request_message[i]))
#print the received arguments
pretty_print_world_qsr_trace(this_qsr[i], qsrlib_response_message[i])
else:
print('didn\'t find the qsr')
#==============================================================================================================#
