Working upon our previous ArUco marker detection post, we can have our drone camera return the distance to the marker plane from the camera feed. We will display it on screen today, although the important use is to include it in an automated flight control loop.
This time we need the following libraries:
import cv2
import numpy as np
from threading import Thread
from djitellopy import Tello
import os
from time import sleep
from time import timeThe OpenCV version we need is the one containing the ArUco marker tools, distributed under opencv-contrib-python. If you had another version installed, the cv2 module name will generate conflicts and not load properly. Watch out for your environments if using Conda or similar, as OpenCV can be grabbed from defaults and result in an error. If ArUco marker tools do not work, you can try uninstalling OpenCV:
!pip3 uninstall opencv-python -y
!pip3 install opencv-contrib-pythonFor simplicity, we will connect ourselves to the TELLO drone first. In this way, we can default our functions to an existing drone connection:
tello = Tello()
tello.connect()
tello.query_battery()We will define a set of functions to control the different parameters of the video stream, marker detection, and information display. First, we start with a function to set the video stream quality:
def set_video_parameters(drone=tello, params=None):
FPS_select = {'high':30}
res_select = {'high': (1280, 720)}
if not params:
FPS_set = 'high'
res_set = 'high'
else:
FPS_set, res_set = params
res_cmd = 'setresolution {}'.format(res_set)
fps_cmd = 'setfps {}'.format(FPS_set)
tello.send_control_command(res_cmd)
tello.send_control_command(fps_cmd)
comp = 'h264'
codec = cv2.VideoWriter_fourcc(*comp)
return FPS_select[FPS_set], codec We set the video capture rate and resolution to the maximum possible by default, send the commands to the drone, and select a codec to compress the video recording. We are only allowing high settings for both capture speed and resolution for the time being.
To highlight the marker in our video feed, we will define the functions that draw a bounding box around the marker and a small circle at its center:
def draw_bbox(img, c):
(x, y, w, h) = cv2.boundingRect(c)
rect = cv2.minAreaRect(c)
box = cv2.boxPoints(rect)
box = np.int0(box)
cv2.drawContours(img,[box],0,RED_COLOR,2)
return
def draw_circle(img, c):
# compute and draw the center of the ArUco
coords = np.int0(c).reshape((4, -1))
topLeft = coords[0]
bottomRight = coords[2]
cX = int((topLeft[0] + bottomRight[0]) / 2.0)
cY = int((topLeft[1] + bottomRight[1]) / 2.0)
cv2.circle(img, (cX, cY), 10, RED_COLOR, -1)
returnWe are using built-in OpenCV tools to draw a rectangle and a circle onto the passed image using the corners of the marker (variable "c"). Our rectangle will not change shape with the tilting angle, also sufficient for the time being. The alternative is drawing four separate lines, corner-to-corner, for the four sides of detected markers.
Finally, the main recording function. We will break down its different parts:
def videoRec(record_mission=False):
# Set video parameters:
FPS, codec = set_video_parameters()
# Start the drone stream:
tello.streamon()
sleep(0.1)
# Grab a frame to check size:
img = tello.get_frame_read().frame
shape = (img.shape[1], img.shape[0])
if record_mission:
i = len(os.listdir(record_dir))
video_file = cv2.VideoWriter(f'{record_dir}/test_flight_{i+1}.mp4',
codec, FPS, shape)
# Default text and detection:
distance_txt = f'Distance: No marker Detected.'
alt_txt = f'Height: No altitude Signal'
marker_on_frame = False
while True:
# Match the frame rate:
sleep(1/FPS)
# Altitude can be obtained without a frame:
altitude = tello.get_height()
alt_txt = f'Height: {altitude}'
img = tello.get_frame_read().frame
c , _, _ = cv2.aruco.detectMarkers(img, arucoDict,
parameters=arucoParams)
if len(c)>0:
marker_on_frame = True
c = np.array(c)
pose = cv2.aruco.estimatePoseSingleMarkers(c.reshape(-1,4,2),
50,
CAMERA_PARAMETERS,
DISTORTION_PARAMETERS)
translation = pose[1]
distance = np.linalg.norm(translation)/1000
distance_txt = f'Distance: {distance:.2f}m'
else:
distance_txt = f'Distance: No marker Detected.'
marker_on_frame = False
# draw the bounding box of the ArUCo detection
for box in c:
if marker_on_frame:
draw_bbox(img, box)
draw_circle(img, box)
info_txt = [alt_txt,
distance_txt]
i = 0
for txt in info_txt:
x = bottomLeftCornerOfText[1]
y = bottomLeftCornerOfText[0]
position = ()
cv2.putText(img, txt,
(y, x+i*50),
font,
fontScale,
fontColor,
lineType)
i = i+1
cv2.imshow ('Drone Video Feed', img)
if record_mission:
video_file.write(img)
# wait for ESC key to exit and terminate feed.
k = cv2.waitKey(1)
if k == 27:
cv2.destroyAllWindows()
if record_mission:
video_file.release()
tello.streamoff()
break
return 0Video recording parameters and feed are initialized, the shape of the first received frame read and recorded, mission recording saved, and distance to marker and height texts initialized to defaults:
# Set video parameters:
FPS, codec = set_video_parameters()
# Start the drone stream:
tello.streamon()
sleep(0.1)
# Grab a frame to check size:
img = tello.get_frame_read().frame
shape = (img.shape[1], img.shape[0])
if record_mission:
i = len(os.listdir(record_dir))
video_file = cv2.VideoWriter(f'{record_dir}/test_flight_{i+1}.mp4',
codec, FPS, shape)
# Default text and detection:
distance_txt = f'Distance: No marker Detected.'
alt_txt = f'Height: No altitude Signal'
marker_on_frame = FalseThis is the main video feed loop. The frame rate is matched to our detection and display rate not to have "twitching" values due to rate and processing speed mismatch. We grab the altitude, the video frame and compute the corners of the AruCo marker, if any. Here we need to add the camera lens intrinsic parameters and distortion. These are constant values for a given camera sensor that we will define before launching the flight mission. From the marker detection corners and the camera parameters, we can determine the pose of the marker. We know the length of its side also so that its apparent size and shape on the camera sensor correspond to the position of the marker in the world outside. The pose estimation function returns the rotation and translation vectors in that order, so our translation vector is pose[1]. The norm of the 3-D translation vector is the distance to the detected marker:
while True:
# Match the frame rate:
sleep(1/FPS)
# Altitude can be obtained without a frame:
altitude = tello.get_height()
alt_txt = f'Height: {altitude}'
img = tello.get_frame_read().frame
c , _, _ = cv2.aruco.detectMarkers(img, arucoDict,
parameters=arucoParams)
if len(c)>0:
marker_on_frame = True
c = np.array(c)
pose = cv2.aruco.estimatePoseSingleMarkers(c.reshape(-1,4,2),
50,
CAMERA_PARAMETERS,
DISTORTION_PARAMETERS)
translation = pose[1]
distance = np.linalg.norm(translation)/1000
distance_txt = f'Distance: {distance:.2f}m'
else:
distance_txt = f'Distance: No marker Detected.'
marker_on_frame = FalseFinally, we draw the bounding box, the circle, and render the texts onto the video frame:
# draw the bounding box of the ArUCo detection
for box in c:
if marker_on_frame:
draw_bbox(img, box)
draw_circle(img, box)
info_txt = [alt_txt,
distance_txt]
i = 0
for txt in info_txt:
x = bottomLeftCornerOfText[1]
y = bottomLeftCornerOfText[0]
position = ()
cv2.putText(img, txt,
(y, x+i*50),
font,
fontScale,
fontColor,
lineType)
i = i+1
cv2.imshow ('Drone Video Feed', img)
if record_mission:
video_file.write(img)
# wait for ESC key to exit and terminate feed.
k = cv2.waitKey(1)
if k == 27:
cv2.destroyAllWindows()
if record_mission:
video_file.release()
tello.streamoff()
break
return 0The constants and colors we need are these, the camera parameters, colors, and overlay parameters for the text:
# Camera Parameters
CAMERA_PARAMETERS = np.array([[921.170702, 0.000000, 459.904354],
[0.000000, 919.018377, 351.238301],
[0.000000, 0.000000, 1.000000]])
DISTORTION_PARAMETERS = np.array([-0.033458,
0.105152,
0.001256,
-0.006647,
0.000000])
# Colors:
RED_COLOR = (255,0,0)
# Text Overlay Parameters:
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (10,500)
fontScale = 1
fontColor = (255,255,255)
lineType = 2This part can fail with incorrect OpenCV Python distributions, the definition of the ArUco marker dictionary, and detection parameters. In our case, our single marker to detect is the 4x4 marker with identification number 0:
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
arucoParams = cv2.aruco.DetectorParameters_create()Everything is set for a manual, no-fly trial. We set the directory to hold our recordings:
record_dir = 'mission_recordings'
try:
os.mkdir(record_dir)
except:
passAnd launch the video recording thread:
#This thread runs receiving video feed.
record_mission = True
receive_video_thread = Thread(target=videoRec, args=[record_mission])
receive_video_thread.daemon = True
receive_video_thread.start()
receive_video_thread.join()In the resulting video, the distance to this specific marker should be displayed on the screen. We are not doing anything of note with this value yet; it could be used as a measuring tool if, for any reason, we have a drone and an ArUco marker and not a ruler at hand...
In future posts, we will use this distance measurement for interactive and automatic flight control.
Do not hesitate to contact us if you require quantitative model development, deployment, verification, or validation. We will also be glad to help you with your machine learning or artificial intelligence challenges when applied to asset management, automation, or intelligence gathering from satellite, drone, or fixed-point imagery.
The Google Colab notebook with this post is here. You may need to download the Jupyter Notebook file and run it locally to connect your TELLO EDU drone.
コメント