ドローンから受信したストリーミング画像に、物体の「運動方向と速度」の可視化処理と、物体の出現位置とラベル名の記入をまとめて行ってみた

取り組んだこと

前回の記事では、MP4の動画ファイルを入力させて、SitarHarmonicsさんの物体の運動方向を可視化する処理に成功しました。今回は。Telloドローンの一人称カメラ画像をストリーミング入力させて、検出物体の動きの方向（＋速度）の可視化のリアルタイム処理をしてみました。

（ 前回の記事 ）

今回作成したスクリプト・ファイルは、GitHubで公開しています。

結果

• Telloをホバリング飛行させて、Tello自身の動きがない状況を作り、被写体の動きの方向を線で可視化できるか実験した
• テレビカメラの正面でホバリングを行い、テレビ画面の中の人物や物体の動きをTelloのカメラに見せた
• ウィンドウに表示される画像は、Telloの動きから5秒ほど遅延した （Macbookの計算速度のスペックの問題）
• MP4ファイルを入力したときは、物体の動きの方向（＋速度）をきれいに可視化できたが、今回はきれいに描画できなかった
• __Telloを着陸させて、完全に静止した状態__で、目の前をボールを転がしたり、手を振ったりすると、きれいに運動ベクトルを可視化した線が描写された。このとき、動いていない物体には線がほとんど描写されなかった。

最後の点を考えると、ホバリング中の　Telloのわずかな（機体自身の）動きが、被写体の動きの方向の描写に乱れを与えてしまうのではないか、と考えられる。

なお、今回作成したコードに、ホバリング中のTelloから送られてくるカメラ画像ではなく、MP4動画ファイルを入力させたところ、物体の移動の向きと速さを可視化した線は、問題なく綺麗に出力されました。

実行中のTerminal画面

Terminal

% python3 tello_motion_vector.py
[INFO] tello.py - 107 - Tello instance was initialized. Host: '192.168.10.1'. Port: '8889'.
[INFO] tello.py - 422 - Send command: 'command'
[INFO] tello.py - 446 - Response command: 'ok'
[INFO] tello.py - 422 - Send command: 'streamon'
[INFO] tello.py - 446 - Response streamon: 'ok'
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] non-existing PPS 0 referenced
[h264 @ 0x7ff6ddffa200] decode_slice_header error
[h264 @ 0x7ff6ddffa200] no frame!
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。

[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。

[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
i

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　i を受信しました。

[INFO] tello.py - 422 - Send command: 'takeoff'
r
[INFO] tello.py - 446 - Response takeoff: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
r

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　r を受信しました。

[INFO] tello.py - 422 - Send command: 'up 30'
[INFO] tello.py - 446 - Response up 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　r を受信しました。

[INFO] tello.py - 422 - Send command: 'up 30'
w
[INFO] tello.py - 446 - Response up 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
w

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　w を受信しました。

[INFO] tello.py - 422 - Send command: 'forward 30'
[INFO] tello.py - 446 - Response forward 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
w

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　w を受信しました。

[INFO] tello.py - 422 - Send command: 'forward 30'
[INFO] tello.py - 446 - Response forward 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
w
[h264 @ 0x7ff6db113c00] error while decoding MB 58 41, bytestream -14

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　w を受信しました。

[INFO] tello.py - 422 - Send command: 'forward 30'
[INFO] tello.py - 446 - Response forward 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
f

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　w を受信しました。

[INFO] tello.py - 422 - Send command: 'forward 30'
[INFO] tello.py - 446 - Response forward 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
f

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　f を受信しました。

[INFO] tello.py - 422 - Send command: 'down 30'
[INFO] tello.py - 446 - Response down 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　f を受信しました。

[INFO] tello.py - 422 - Send command: 'down 30'
p
[h264 @ 0x7ff6db113c00] left block unavailable for requested intra mode
[h264 @ 0x7ff6db113c00] error while decoding MB 0 13, bytestream 14827
[INFO] tello.py - 446 - Response down 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　p を受信しました。

フレーム画像を保存しました。
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'
p
[h264 @ 0x7ff6ddff2200] error while decoding MB 13 42, bytestream -14
r
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　p を受信しました。

フレーム画像を保存しました。
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'


0.1秒以内に操作コマンドを入力して下さい :
操作コマンド：　r を受信しました。

[INFO] tello.py - 422 - Send command: 'up 30'
[INFO] tello.py - 446 - Response up 30: 'ok'
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '22'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。

[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '21'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。

[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '21'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。

[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '21'
p
0.1秒以内に操作コマンドを入力して下さい :

操作コマンド：　p を受信しました。

フレーム画像を保存しました。
[INFO] tello.py - 422 - Send command: 'battery?'
[INFO] tello.py - 446 - Response battery?: '20'

0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。

（ 省略 ）

作成したスクリプト・ファイル

他に、DJITelloPyのファイル資源など、必要なものがあります。_
必要な資源一式をGitHubに上げましたので、git cloneしてください。

common_7.py

from __future__ import absolute_import, division, print_function
import sys, cv2, math, time, datetime, os, glob, copy
import numpy as np
import matplotlib.pyplot as plt
import matplotlib as mpl
import matplotlib.cm as cm
import cvlib as cvl
import PIL.Image as pil
import torch
from cvlib.object_detection import draw_bbox
from timeout_decorator import timeout, TimeoutError
from djitellopy import Tello
from pprint import pprint
from create_caption_text import *
from get_mask_rcnn_image import *
from depth_frame import *
from create_motion_vector_image import *
from torchvision import transforms, datasets
from timeout_decorator import timeout, TimeoutError
from djitellopy import Tello


# Esc キー
ESC_KEY = 0x1b
# モーションの残存期間(sec)
DURATION = 1.0
# 全体の方向を表示するラインの長さ
LINE_LENGTH_ALL = 60
# 座標毎の方向を表示するラインの長さ
LINE_LENGTH_GRID = 20
# 座標毎の方向を計算する間隔
GRID_WIDTH = 40
# 方向を表示するラインの丸の半径
CIRCLE_RADIUS = 2

create_motion_vector_image.py

import numpy as np
import cv2

# オプティカルフロー解析描画
def drawOptFlow(img, gray, flow, step=16, dispsc=10):
    cimg    = img.copy()
    h, w    = img.shape[:2]
    y, x    = np.mgrid[step/2:h:step, step/2:w:step].reshape(2,-1).astype(int)
    dx, dy  = flow[y,x].T * dispsc
    dist = np.sqrt(dx**2+dy**2)
    idx     = np.where(3 < dist)
    x, y    = x[idx], y[idx]
    dx, dy  = dx[idx], dy[idx]
    lines = np.vstack([x, y, x+dx, y+dy]).T.reshape(-1, 2, 2)
    lines = lines.astype(np.int32)
    fx, fy = flow[y,x].T
    cv2.polylines(cimg, lines, False, (255, 0, 255), 1, 8)
    return cimg

def get_motion_vector_img(frame, prvs):
    # 縮小画像取得
    frame2 = cv2.resize(frame, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)
    next = cv2.cvtColor(frame2,cv2.COLOR_BGR2GRAY)

    # オプティカルフロー解析
    flow = cv2.calcOpticalFlowFarneback(prvs, next, None, 0.5, 3, 15, 3, 5, 1.2, 0)

    # オプティカルフロー解析描画
    rgb2 = drawOptFlow(frame2, next, flow, 16)
    
    return rgb2, next

tello_motion_vector.py

from common_7 import *

TIMEOUT_SEC = 0.1

@timeout(TIMEOUT_SEC)
def input_with_timeout(msg=None):
   return input(msg)

tello = Tello()
tello.connect()

tello.streamon()
frame_read = tello.get_frame_read()
img = frame_read.frame
# motion vector可視化
# 縮小画像取得
frame1 = cv2.resize(img, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)
prvs = cv2.cvtColor(frame1,cv2.COLOR_BGR2GRAY)

hsv = np.zeros_like(frame1)
hsv[...,1] = 255

while True:
    img = frame_read.frame
    image = img.copy()
    image2 = img.copy()

    # motion vector可視化
    rgb2, next = get_motion_vector_img(img, prvs)

    # Telloの現在高度（ToFセンサ計測距離(cm)、高さ（cm)）とバッテリ残量を取得する
    #https://djitellopy.readthedocs.io/en/latest/tello/#djitellopy.tello.Tello.query_battery
    time_of_flight_distance_senser_val = tello.get_distance_tof()
    input_text_1 = "ToF Distane {0} cm".format(time_of_flight_distance_senser_val)
    
    height = tello.get_height()
    input_text_2 = "Height {0} cm".format(height)
    
    bettery = tello.query_battery()
    input_text_3 = "Battery {0} %".format(bettery)
    # ウィンドウのサイズが小さいので、大きくする
    height = rgb2.shape[0]
    width = rgb2.shape[1]
    motion_vector_img = cv2.resize(rgb2, (int(3.0*width), int(3.0*height)))

    prvs = next

    # 物体検出矩形表示と人物検出人数の文字列埋込み表示の画像を取得
    # 物体の運動ベクトルを線で書き込んだ画像（motion_vector_img)ではなく、
    # 何も書き込まれていないカメラ画像(image)をもとに解析を行う。
    # motion_vector_imgとimageは同じタイミングでTelloから取得済み。
    
    label_name  = "person"
    bbox, label, conf = cvl.detect_common_objects(image)
    obejct_detected_img = draw_bbox(image, bbox, label, conf)
    
    input_text_4 = "Num of detected {0}(s) is {1}".format(label_name, str(label.count(label_name)))
    
    # 2つの画像、motion_vector_imgとobejct_detected_imgを重ね合わせる
    # 最初に、2つの画像の縦横サイズを揃える
    height_0 = motion_vector_img.shape[0]
    width_0 = motion_vector_img.shape[1]
    resized_obejct_detected_img = cv2.resize(obejct_detected_img, (width_0, height_0))
    # 同じサイズになった2つの画像を重ね合わせる
    # https://atatat.hatenablog.com/entry/opencv3_overlay
    # https://code-graffiti.com/blending-images-with-opencv-in-python/
    blended_img = cv2.addWeighted(src1=motion_vector_img, alpha=0.5, src2=resized_obejct_detected_img, beta=0.5, gamma=0)

    # 画像にTelloの現在高度（ToFセンサ計測距離(cm)、高さ（cm)）とバッテリ（%）を埋込む
    cv2.putText(blended_img, str(input_text_1), (0, 100), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
    cv2.putText(blended_img, str(input_text_2), (0, 150), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
    cv2.putText(blended_img, str(input_text_3), (0, 200), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
    # カメラ画像に物体検出矩形表示と人物検出人数の文字列埋込み表示を埋込む
    cv2.putText(blended_img, str(input_text_4), (0, 50), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)

    # 重ね合わせた画像を1.7倍に縦横同じ比率で拡大する
    height_1 = blended_img.shape[0]
    width_1 = blended_img.shape[1]
    resized_blended_img = cv2.resize(blended_img, (int(1.7*width_1), int(1.7*height_1)))

    # Escキー押下で終了
    if cv2.waitKey(20) == ESC_KEY:
        break

    # 1.7倍に拡大した画像をウィンドウに出力する
    cv2.namedWindow("Blended", cv2.WINDOW_NORMAL)
    cv2.imshow("Blended", resized_blended_img)
    
    #次の行（key = cv2.・・・）を削除すると、画像が受信できなくなる。
    key = cv2.waitKey(1) & 0xff
    
    try:
        msg = input_with_timeout('\n{}秒以内に操作コマンドを入力して下さい :'.format(TIMEOUT_SEC))
        print('\n操作コマンド：　{} を受信しました。\n'.format(msg))
        if msg == "i":
            tello.takeoff()
        elif msg == "w":
            tello.move_forward(30)
        elif msg == "s":
            tello.move_back(30)
        elif msg == "a":
            tello.move_left(30)
        elif msg == "d":
            tello.move_right(30)
        elif msg == "e":
            tello.rotate_clockwise(30)
        elif msg == "q":
            tello.rotate_counter_clockwise(30)
        elif msg == "r":
            tello.move_up(30)
        elif msg == "f":
            tello.move_down(30)
        elif msg == "g":
            tello.land()
        elif msg == "p":
            dt_now = datetime.datetime.now()
            timestamp_str = dt_now.strftime('%Y年%m月%d日%H:%M:%S')
            file_name = "frame_img_shot_{0}.jpg".format(timestamp_str)
            cv2.imwrite(file_name, resized_blended_img)
            print("フレーム画像を保存しました。")
    except TimeoutError:
        print('\n操作コマンド入力時間切れ。次のフレーム画像を読み込みます。\n')

tello.land()