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