Automated Drone Flight: Real-Time Distance to Target

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 time

The 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-python

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

We 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 0

Video 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 = False

This 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 = False

Finally, 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 0

The 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               = 2

This 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:
    pass

And 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.


16 views0 comments

Recent Posts

See All