task1.py 文件源码

def _find_control(self, tag, mask, details, header, factor, other_one=None):
        """ Find a control """

        corners = [None, None]
        indexes = mask.nonzero()
        points = []
        indexes2 = []
        for i in range(len(indexes[1])):
            pnt = details[indexes[0][i], indexes[1][i]]
            if pnt[2] > DISH_MAXIMUM:
                print 'rejecting point, too distant ', pnt[2]
                mask[indexes[0][i], indexes[1][i]] = 0
                continue
            if other_one:
                dist = np.sqrt((other_one[0].point.y - pnt[0])**2 + \
                               (other_one[0].point.z - pnt[1])**2 +\
                               (other_one[0].point.x - pnt[2])**2)
                if dist > 1.0:
                    print 'rejecting point, too far away from other one', dist
                    mask[indexes[0][i], indexes[1][i]] = 0
                    continue
            points.append(details[indexes[0][i], indexes[1][i]])
            indexes2.append((indexes[0][i], indexes[1][i]))
        points = np.array(points)

        if len(points) < 3:
            log("Error: only {} {} points found.".format(len(points), tag))
            raise ZarjConfused('Error: only {} {} points found.'.format(
                len(points), tag))

        mean = np.mean(points, axis=0)[2]
        stdv = np.std(points, axis=0)[2]

        for i, pnt in enumerate(points):
            if abs(pnt[2] - mean) < factor * stdv:
                _add_bounding_point(pnt, corners)
            else:
                mask[indexes[0][i], indexes[1][i]] = 0

        point = PointStamped()
        point.header = header
        point.point.x = corners[0][0]
        point.point.y = corners[0][1]
        point.point.z = corners[0][2]
        cor1 = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')

        point = PointStamped()
        point.header = header
        point.point.x = corners[1][0]
        point.point.y = corners[1][1]
        point.point.z = corners[1][2]
        cor2 = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')

        log("{}, {} {}".format(tag, cor1, cor2))

        return (cor1, cor2)