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)
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)