예제 #1
0
    def idepth_stats(self):
        # compute some interesting things given a matrix
        # of "interesting" distances (noninteresting distances are 0)
        # idepth is a 240 x 320 matrix of depth data
        depth_ixs = numpy.nonzero(self.interesting_depths)
        depth_to_xyz2(depth=self.depth, xyz=self.xyz)
        count = len(depth_ixs[0])
        if count != 0:
            sum = numpy.sum(self.interesting_depths[depth_ixs])
            self.avg_mm = sum / count
        else:
            avg_mm = -1
        rects = [(cv2.boundingRect(c), c) for c in self.contours]
        self.targets = self.build_targets(rects)

        chosen_j = self.choose_target()

        for j, target in enumerate(self.targets):
            if j == chosen_j:
                target.draw_color = (0, 255, 0)
            target.draw(self.contour_img)
            self.target_mask[:] = 0
            cv2.drawContours(
                self.target_mask,
                target.contours, -1, (0xffff), cv2.FILLED)
            cv2.bitwise_and(
                self.depth, self.target_mask, dst=self.target_depths)
            depth_ixs = numpy.nonzero(self.target_depths)
            count = len(depth_ixs[0])
            if count != 0:
                sum = numpy.sum(self.target_depths[depth_ixs])
                target.avg_mm = sum / count
                target.theta = h_angle(target.center_x, CX=self.center_x)
                target.theta_v = v_angle(target.center_y, CY=self.center_y)

            if j == chosen_j:
                self.sd.putNumber("target_dist", target.avg_mm)
                self.sd.putNumber("target_theta", target.theta)
                self.sd.putNumber("target_theta_v", target.theta_v)
                cv2.putText(self.contour_img,
                    ("d= %.2f in" % target.avg_in), (10, 40),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255))
                cv2.putText(self.contour_img,
                    ("theta= %.2f" % target.theta), (10, 60),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255))
                cv2.putText(self.contour_img,
                    ("thetav= %.2f" % target.theta_v), (10, 80),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255))
            else:
                self.sd.putNumber("target_dist", -1)
                self.sd.putNumber("target_theta", 1000)
                self.sd.putNumber("target_theta_v", 1000)


        self.predict_impact(chosen_j)
        self.measure_target(chosen_j)
예제 #2
0
    def test_convert_to_xyz2(self):
        depths = numpy.empty(shape=(240, 320), dtype='uint16')
        xyz = numpy.zeros(shape=(3, 240, 320), dtype='float32')
        depths[:, :] = 1766
        xyz = depth_to_xyz2(depths, xyz)
        self.assertAlmostEqual(xyz[0, 120, 160], 0)
        self.assertAlmostEqual(xyz[1, 120, 160], 0)
        self.assertAlmostEqual(xyz[2, 120, 160], 1766)

        self.assertAlmostEqual(xyz[0, 120, 0], -866.63322, 4)
        self.assertAlmostEqual(xyz[1, 120, 0], 0)
        self.assertAlmostEqual(xyz[2, 120, 0], 1538.7341, 4)

        self.assertAlmostEqual(xyz[0, 120, 319], 861.69586, 4)
        self.assertAlmostEqual(xyz[1, 120, 319], 0)
        self.assertAlmostEqual(xyz[2, 120, 319], 1541.5045, 4)

        self.assertAlmostEqual(xyz[0, 0, 160], 0)
        self.assertAlmostEqual(xyz[1, 0, 160], 687.1793, 4)
        self.assertAlmostEqual(xyz[2, 0, 160], 1626.8192, 4)

        self.assertAlmostEqual(xyz[0, 239, 160], 0)
        self.assertAlmostEqual(xyz[1, 239, 160], -681.7571, 4)
        self.assertAlmostEqual(xyz[2, 239, 160], 1629.0989, 4)
        
        self.assertAlmostEqual(xyz[0, 0, 0], -798.3327, 4)
        self.assertAlmostEqual(xyz[1, 0, 0], 687.17926, 4)
        self.assertAlmostEqual(xyz[2, 0, 0], 1417.4645, 4)