def inst(self, table_size, ground_objects_on_table=False, ground_bb_axis=SolidPrimitive.BOX_Z,
yaw_only_on_table=False):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type.name
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.on_table = 0 < inst.pose.position.x < table_size[0] and 0 < inst.pose.position.y < table_size[1]
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
q_arr = transformations.quaternion_from_euler(*cur_rpy)
# ground objects that are really sitting on the table (exclude those in the air)
if inst.on_table and ground_objects_on_table and np.average(
pz, weights=w) < self.object_type.bbox.dimensions[ground_bb_axis] / 2.0 + 0.1:
# TODO consider orientation!
inst.pose.position.z = self.object_type.bbox.dimensions[ground_bb_axis] / 2.0
if yaw_only_on_table:
# TODO figure out which axis should be kept
# ...like this it only works for some objects (containers)
q_arr[0] = 0.0
q_arr[1] = 0.0
q_arr = transformations.unit_vector(q_arr)
else:
inst.pose.position.z = np.average(pz, weights=w)
a2q(inst.pose.orientation, q_arr)
for (key, value) in self.flags.iteritems():
kv = KeyValue()
kv.key = key
kv.value = value
inst.flags.append(kv)
return inst
def inst(self):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.pose.position.z = np.average(pz, weights=w)
# TODO read table size from param
inst.on_table = 0 < inst.pose.position.x < 1.5 and 0 < inst.pose.position.x < 0.7
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy))
# TODO "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees
# yaw (in table coordinates) may stay as it is
for (key, value) in self.flags.iteritems():
kv = KeyValue()
kv.key = key
kv.value = value
inst.flags.append(kv)
return inst