マジでTrackingやってみた。
当初は、まんままねっこだけで行けるはずだったが、どうもTrackingは難しい。
⓪そもそも環境構築できないし、参考サイトのものが動かない。。。
①動画がリアルタイムに表示されない
②ロジック的にリアルタイムで動作するものを安定させるのは難しい
③Trackingすると暴走する。。方向や距離がうまく適切に動かない
④Cv2.tracker(枠)がはずれる
⑤毎日１０分くらいしか試せない
。。。
ということで、まだまだ未完成だがある程度収束（安定飛行）してきたので、まとめて置こうと思う。

【参考】
１．トイドローン Tello をプログラミングで機能拡張！顔認識と自動追尾を実装してみた
２．Object Tracking using OpenCV (C++/Python)
３．stackoverflow「AttributeError: module 'cv2.cv2' has no attribute 'createLBPHFaceRecognizer'」

一応の目標

①とにかく飛行させる
②GamePadで飛ばす
③PCで飛ばす
④Pythonで飛ばす
⑤画像入力する
⑥物体検出搭載
⑦トラッキングする
⑧物体を探して見つける
⑨...
今回やったこと
⑦~~物体識別機能付き~~マジ・トラッキングする
当初は、物体識別機能つけたけど、やはりまだまだ時期早々な気がして削除しました。
・環境構築
・CV2.Tracker
・TelloでTracking：そしてTracking全体のロジックは。。

・環境構築

今回は以下の環境で実施しています。
・conda環境
　構築は、ancondaインストールします。
・Trackerをインストール
これは、参考１を動かそうとすると動きません（まあ、Macなので？？）
ではなくて、。。。どうやら参考２にたどり着きます。
そして、このコードも動きません。
しかし、エラーをググると・。

>python CV2_Tracking.py
Traceback (most recent call last):
  File "CV2_Tracking.py", line 22, in <module>
    tracker = cv2.TrackerMIL_create()
AttributeError: module 'cv2.cv2' has no attribute 'TrackerMIL_create'

参考３にたどり着きます・。
そして、以下のインストールをしました。
動きました・。

>pip install opencv-contrib-python
Collecting opencv-contrib-python
  Downloading https://files.pythonhosted.org/packages/1b/97/f0ecbf3560877b2d923e3a706a5bf89167d0827c69372f94e857f5b177f9/opencv_contrib_python-3.4.4.19-cp35-cp35m-win_amd64.whl (44.1MB)
    100% |################################| 44.1MB 385kB/s
Requirement already satisfied: numpy>=1.11.1 in c:\users\tosio\anaconda3\lib\site-packages (from opencv-contrib-python) (1.14.5)
Installing collected packages: opencv-contrib-python
Successfully installed opencv-contrib-python-

CV2.Tracker

そして参考２のコード（おまけに記載）は以下のとおりで、まんま動きます。
・画像が出たらTrackingしたい部分をマウスでくくって、press SPACE or ENTER button!
・ESCキーで終了します。

>python CV2_Tracking.py
Select a ROI and then press SPACE or ENTER button!
Cancel the selection process by pressing c button!

顔を抽出するとこの参考２の動画のようにTrackingしてくれます。
ｆｐｓも表示上３０-100位は出ているようです。
ただし、顔はよく追従しますが、そのほかのものはTracking出来ないことが多いように思います。

【参考】
・How to select a bounding box ( ROI ) in OpenCV (C++/Python) ?
・Rect2d cv::selectROI (Mat img, bool fromCenter=true)

そしてTracking全体のロジックは。。

全体のロジックを紆余曲折の結果以下のとおりとしました。

コードは以下に置きました

・Tello/Tello_waitkey.py
不足しているものは、インストールします。
最後のversionも以下のコードを変更したくないので、そのまま残しました。

import cv2
import tellopy
import numpy
import av
import math as m
from time import sleep

(major_ver, minor_ver, subminor_ver) = (cv2.__version__).split('.')

Batの残量を表示するために残しました。

def handler(event, sender, data, **args):
    drone = sender
    if event is drone.EVENT_FLIGHT_DATA:
        print(data)

以下がTrackingを判断するコードです。
距離ｄはCV2.Trackingだと枠の大きさが変わらないので、変化しないので判断を切りました。
Camshiftなら、大きさに合わせて枠が変わるので。そのうちやりたいと思います。
※左右と上下はどのように座標を見ているかによるので、以下で正しいと思っていますが、。。

def tracking(drone,d,dx,dy,LB):
    """
    if (d - LB) > 15:
        drone.set_pitch(5)   #drone.pitch = drone.STICK_HOVER + drone.STICK_L
        #sleep(1)
    elif (d - LB) < -15:
        drone.set_pitch(-5)   #drone.pitch = drone.STICK_HOVER - drone.STICK_L
        #sleep(1)
    """    
    if dx > 5:  #80
        drone.right(5)    #drone.roll = drone.STICK_HOVER + drone.STICK_L
        #sleep(1)
    elif dx < -5:#-80
        drone.left(5)    #drone.roll = drone.STICK_HOVER - drone.STICK_L
        #sleep(1)
    elif dy > 5:
        drone.up(5)    #drone.thr = drone.STICK_HOVER - drone.STICK_L
        #sleep(1)
    elif dy < -5:
        drone.down(5)    #drone.thr = drone.STICK_HOVER + drone.STICK_L
        #sleep(1)

以下は、キー操作の場合の関数です。これもどの程度の大きさが一番制御しやすいかに関係します。これは以下で一応動くので良しとします。

def key_Operation(drone,key):
    if key == ord('n'):  #n
        drone.down(10)
        sleep(1)
    elif key==ord('u'):  #117:  #u
        drone.up(10)
        sleep(1)
    elif key==ord('h'):  #104:  #h
        drone.left(5)
        sleep(1)
    elif key==ord('j'):  #106:  #j
        drone.right(5)
        sleep(1)
    elif key==ord('b'):  #backward
        drone.backward(5)
        sleep(1)
    elif key==ord('f'):  #forward
        drone.forward(5)
        sleep(1)
    elif key==ord('c'):  #clockwise
        drone.clockwise(10)
        sleep(1)

いよいよ本体です。
最初のblockはTrackerの手法の定義でそれぞれ精度が異なりました。
今回は３番目のKCFを利用します。

def main():
    tracker_types = ['BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN', 'MOSSE', 'CSRT'] 
    tracker_type = tracker_types[2]
     
    if int(minor_ver) < 3:
        tracker = cv2.Tracker_create(tracker_type)
    else:
        if tracker_type == 'BOOSTING':
            tracker = cv2.TrackerBoosting_create()
        if tracker_type == 'MIL':
            tracker = cv2.TrackerMIL_create()
        if tracker_type == 'KCF':
            tracker = cv2.TrackerKCF_create()
        if tracker_type == 'TLD':
            tracker = cv2.TrackerTLD_create()
        if tracker_type == 'MEDIANFLOW':
            tracker = cv2.TrackerMedianFlow_create()
        if tracker_type == 'GOTURN':
            tracker = cv2.TrackerGOTURN_create()
        if tracker_type == 'MOSSE':
            tracker = cv2.TrackerMOSSE_create()
        if tracker_type == "CSRT":
            tracker = cv2.TrackerCSRT_create()

Trackerの枠の座標等の初期値を定義します。bboxがTrackerの枠です。

    # 追跡する枠の座標とサイズ
    x = 200
    y = 200
    w = 224
    h = 224
    track_window=(x,y,w,h)
    # Reference Distance
    L0 = 100
    #S0 = 50176 #224x224

    # Base Distance
    LB = 100        
    # Define an initial bounding box
    bbox = (x, y, w, h)   #(287, 23, 86, 320)

ドローンの通信開始し、まず飛行させます。
最初はroiで追跡物体を決めたいので、キー制御で飛ばします。
※実は当初は飛ばす前に追跡物体決めていましたが、それだと画像の時間差が大きくなりすぎてTrackingは出来ません。

    drone = tellopy.Tello()
    drone.connect()
    container = av.open(drone.get_video_stream()) 
    drone.takeoff()
    #drone.is_autopilot="True"
    drone.is_autopilot="False"

while True:で永続化します。
for frame in container.decode(video=0):で画像をframeに入力します。
※参考１のudpでアドレス指定して飛ばすこともやってみましたが、うまくいきませんでした。

    while True:
        for frame in container.decode(video=0):
            image = cv2.cvtColor(numpy.array(frame.to_image()), cv2.COLOR_RGB2BGR)
            # Start timer
            timer = cv2.getTickCount()
            # Update tracker
            ok, bbox = tracker.update(image)
            # Calculate Frames per second (FPS)
            fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);

bboxの値は枠の (x,y,w,h) = (int(bbox[0]),int(bbox[1]),int(bbox[2]),int(bbox[3]))になっています。
そして、imageに枠または"Tracking failure detected"を表示します。
さらにtracker_typeとfpsを表示します。

            # Draw bounding box
            if ok:
                (x,y,w,h) = (int(bbox[0]),int(bbox[1]),int(bbox[2]),int(bbox[3]))
                CX=int(bbox[0]+0.5*bbox[2])
                CY=int(bbox[1]+0.5*bbox[3])
                S0=bbox[2]*bbox[3]
                print("CX,CY,S0=",CX,CY,S0)
                # Tracking success
                p1 = (x, y)
                p2 = (x + w, y + h)
                cv2.rectangle(image, p1, p2, (255,0,0), 2, 1)
            else :
                # Tracking failure
                cv2.putText(image, "Tracking failure detected", (100,80), cv2.FONT_HERSHEY_SIMPLEX, 0.75,(0,0,255),2)
            # Display tracker type on frame
            cv2.putText(image, tracker_type + " Tracker", (100,20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50,170,50),2)
     
            # Display FPS on frame
            cv2.putText(image, "FPS : " + str(int(fps)), (100,50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50,170,50), 2)
            cv2.imshow('test',image)

以下で、keyにより自動制御かキー制御かを決めます。

            key = cv2.waitKey(1)&0xff
            if key == ord('a'):
                drone.is_autopilot="True"
            elif key == ord('s'):
                drone.is_autopilot="False"

もし自動制御の場合は距離dを計算し、dx,dyを計算して、上記のTracking関数を呼び出します。
一方、キー制御の場合は、上記のキー制御関数で制御します。

            if drone.is_autopilot=="True":
                d = round(L0 * m.sqrt(S0 / (w * h)))
                dx = x + w/2 - CX
                dy = y + h/2 - CY
                print(d,dx,dy,drone.is_autopilot,w,h)
                tracking(drone,d,dx,dy,LB)
            elif drone.is_autopilot=="False":
                key_Operation(drone,key)
                print("key=",key,ord('q'))

以下、キー入力がqなら終了、rならcv2.selectROI(image, False)を呼び出します。roiではターゲットとする物体をマウスでくくって、enterキーを押すとターゲットが決まります。
※ここで初めて実際のbboxが定義されます

最後のbreakはwhile文から脱出するために必要です。

            print("key=",key,ord('q'))
            if key == ord('q'):
                cv2.destroyAllWindows()
                break
            elif key == ord('r'):
                bbox = cv2.selectROI(image, False)
                print(bbox)
                (x,y,w,h) = (int(bbox[0]),int(bbox[1]),int(bbox[2]),int(bbox[3]))
                # Initialize tracker with first frame and bounding box
                ok = tracker.init(image, bbox)
                
        break

以下、着陸の作法です。
drone.subscribe(drone.EVENT_FLIGHT_DATA, handler) はBatteryなどの情報を出力します。

    drone.down(50)
    sleep(5)
    drone.land()    
    drone.subscribe(drone.EVENT_FLIGHT_DATA, handler)    
    drone.quit()

以下、起動のおまじないです。

if __name__ == '__main__':
    main()

まとめ

・Telloで追尾をやってみた
・キー制御から追跡モードへの切り替えができた
・不安定だが、ほぼリアルタイム動画で追跡できた

・UDPによる動画取得が出来なかった
・キー操作でTrackingは安定しているが、自動追尾モードが不安定である

おまけ

import cv2
import sys

(major_ver, minor_ver, subminor_ver) = (cv2.__version__).split('.')
 
if __name__ == '__main__' :
    # Set up tracker.
    # Instead of MIL, you can also use
    tracker_types = ['BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN', 'MOSSE', 'CSRT'] 
    tracker_type = tracker_types[2]

    if int(minor_ver) < 3:
        tracker = cv2.Tracker_create(tracker_type)
    else:
        if tracker_type == 'BOOSTING':
            tracker = cv2.TrackerBoosting_create()
        if tracker_type == 'MIL':
            tracker = cv2.TrackerMIL_create()
        if tracker_type == 'KCF':
            tracker = cv2.TrackerKCF_create()
        if tracker_type == 'TLD':
            tracker = cv2.TrackerTLD_create()
        if tracker_type == 'MEDIANFLOW':
            tracker = cv2.TrackerMedianFlow_create()
        if tracker_type == 'GOTURN':
            tracker = cv2.TrackerGOTURN_create()
        if tracker_type == 'MOSSE':
            tracker = cv2.TrackerMOSSE_create()
        if tracker_type == "CSRT":
            tracker = cv2.TrackerCSRT_create()
 
    # Read video
    #video = cv2.VideoCapture("chaplin.mp4") #動画
    video = cv2.VideoCapture(0)　　#PCのカメラ
 
    # Exit if video not opened.
    if not video.isOpened():
        print( "Could not open video")
        sys.exit()
 
    # Read first frame.
    ok, frame = video.read()
    if not ok:
        print( 'Cannot read video file')
        sys.exit()
     
    # Define an initial bounding box
    bbox = (287, 23, 86, 320)
 
    # Uncomment the line below to select a different bounding box
    bbox = cv2.selectROI(frame, False)
 
    # Initialize tracker with first frame and bounding box
    ok = tracker.init(frame, bbox)
        
    while True:
        
        # Read a new frame
        ok, frame = video.read()
        if not ok:
            break
         
        # Start timer
        timer = cv2.getTickCount()
 
        # Update tracker
        ok, bbox = tracker.update(frame)
 
        # Calculate Frames per second (FPS)
        fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
        
        # Draw bounding box
        if ok:
            # Tracking success
            p1 = (int(bbox[0]), int(bbox[1]))
            p2 = (int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3]))
            cv2.rectangle(frame, p1, p2, (255,0,0), 2, 1)
        else :
            # Tracking failure
            cv2.putText(frame, "Tracking failure detected", (100,80), cv2.FONT_HERSHEY_SIMPLEX, 0.75,(0,0,255),2)
 
        # Display tracker type on frame
        cv2.putText(frame, tracker_type + " Tracker", (100,20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50,170,50),2);
     
        # Display FPS on frame
        cv2.putText(frame, "FPS : " + str(int(fps)), (100,50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50,170,50), 2);
 
        # Display result
        cv2.imshow("Tracking", frame)
 
        # Exit if ESC pressed
        k = cv2.waitKey(1) & 0xff
        if k == 27 : break