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 get_angle(y_pixel):
return self.angle + v_angle(y_pixel, CY=self.center_y)
