Exemplo n.º 1
0
    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
Exemplo n.º 2
0
    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