def detect(self, img, x=0, y=0, w=0, h=0):
detector = tello_center.get_preloaded(
YoloService.PRELOAD_YOLO_DETECTOR) # type: Detect
if detector is None:
raise BaseException('detector not loaded')
if w == 0 or h == 0:
return self.post_detect(img)
else:
c = np.copy(img[y:y + h, x:x + w])
locks.imshow('copy', c)
results = self.post_detect(c)
if results is not None:
for r in results:
r.x1 += x
r.x2 += x
r.y1 += y
r.y2 += y
else:
results = []
# for r in detector.detect(img) or []:
# results.append(r)
if len(results) == 0:
return None
return results
def draw_result(self, img, det, show=False):
if det is None or len(det) == 0:
if show:
locks.imshow('result', img)
return
for det_pack in det:
xyxy = []
for c in [det_pack.x1, det_pack.y1, det_pack.x2, det_pack.y2]:
xyxy.append(c)
conf = det_pack.class_conf
label = '%s %.2f %.2f' % (det_pack.class_name, conf,
det_pack.object_conf)
plot_one_box(xyxy, img, label=label, color=det_pack.color)
if show:
locks.imshow('result', img)
def loop_thread(self):
while self.flag():
detector = tello_center.get_preloaded(
YoloService.PRELOAD_YOLO_DETECTOR) # type: Detect
if self.backend.available() and detector is not None:
img, _ = self.backend.drone.get_image_and_state()
if img is None:
time.sleep(0.1)
continue
img = np.copy(img)
result = self.detect(img)
#result = self.detect(img, 480, 100, 480, 360)
detector.draw_result(img, result)
locks.imshow('detect', img)
time.sleep(0.3)
else:
time.sleep(0.1)
self.logger.info('exit loop detection')
def main_loop(self):
image_process_fps = tello_center.FpsRecoder(
ImageProcessService.KEY_FPS)
preid = self.id
while True:
if self.id > preid:
image_process_fps.on_loop()
preid = self.id
showimg = None
self.lock.acquire()
try:
image, state = self.image, self.state
finally:
self.lock.release()
if image is not None and state is not None:
image = np.copy(image)
showimg = np.copy(image)
else:
continue
if image is not None and state is not None and showimg is not None:
self.do_process(image, state, showimg)
locks.imshow(ImageProcessService.name, showimg)
else:
time.sleep(0.001)
def process(self, image, state: tello_data.TelloData, showimg):
x, y, w, h = redball_detecter.find_red_ball(image)
if x is not None:
cv2.rectangle(showimg, (x, y), (x + w, y + h), (255, 0, 0),
thickness=2)
px = x + w / 2.0
py = y + h / 2.0
view = 45 / 180.0 * np.pi
det = 10 / 180.0 * np.pi
_y, _dis_y, _det_y = solve_system(det, (np.pi - view) / 2, view,
720, x, x + h, 10)
pix_size = 1.0 / w * 10 # 单位像素对应的实际长度
la_x = 480
la_y = 168
cx = int(x + w / 2)
cy = int(y + h / 2)
rh = _y(la_y) - _y(cy) # (360 - cy) * pix_size
# delta = 20
# ry1 = _y(la_y)
# ry2 = _y(la_y + delta)
# scale = (ry2 - ry1) / delta
ry = (cx - la_x) * pix_size # (cx - 480) * pix_size
ry += (180 - state.x) * np.tan(state.mpry[1] / 180.0 * np.pi)
cv2.line(showimg, (la_x, la_y), (cx, la_y), (0, 255, 0),
thickness=2)
cv2.line(showimg, (cx, la_y), (cx, cy), (0, 255, 0), thickness=2)
s = "y offset: %.2fcm" % ry
cv2.putText(showimg,
s, (la_x, la_y),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 0, 0),
thickness=5)
cv2.putText(showimg,
s, (la_x, la_y),
cv2.FONT_HERSHEY_PLAIN,
1, (255, 255, 255),
thickness=1)
s = "h offset: %.2fcm" % rh
cv2.putText(showimg,
s, (cx, la_y + 20),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 0, 0),
thickness=5)
cv2.putText(showimg,
s, (cx, la_y + 20),
cv2.FONT_HERSHEY_PLAIN,
1, (255, 255, 255),
thickness=1)
pos = state.get_pos()
ray = state.get_ray(px, py)
if self.world.available():
pos = self.world.collide_ray_window(pos, ray)
if pos is not None:
pos = pos[0]
# print(pos)
self.detectedPos.append(pos)
if self.pos_handler is not None:
self.pos_handler(pos)
s = '%.2f %.2f %.2f' % (pos[0], pos[1], pos[2])
la_x = 960 - 200
la_y = 720 - 30
cv2.putText(showimg,
s, (la_x, la_y),
cv2.FONT_HERSHEY_PLAIN,
1, (0, 0, 0),
thickness=5)
cv2.putText(showimg,
s, (la_x, la_y),
cv2.FONT_HERSHEY_PLAIN,
1, (255, 255, 255),
thickness=1)
if tello_center.get_config(
FireDetector.CONFIG_SHOW_POS_MAP, fallback=False):
fmap = np.array([206, 300, 3])
locks.imshow('fire_map', fmap)
self.preload.put_loaded(FireDetector.PRELOAD_FIRE_POS,
np.copy(pos))
elif self.pos_handler is not None:
self.pos_handler(None)
def on_new_frame(self, img, state):
if self.frame is None or self.frame is not img:
self.frame = img
locks.imshow('frame', self.frame)
self.out.write(self.frame)