Help us understand the problem. What is going on with this article?

色検出プログラム

はじめに

このページは,

公式SDK「Tello-Python」を試そう

の1ページです.
全体を見たい場合は上記ページへお戻りください.

概要

画像処理で物体検出,といえば「二値化・ラベリング・面積&重心計算」が古くから定番です.

最近はそんなレガシー(伝統的・遺産的)な前処理なんか省いて,ディープ・ラーニングで検出してしまう「Semantic Segmentation」の方が流行しています.性能もすごいし...

  参考(動画を再生):http://mprg.jp/research/segmentation_e

とはいえ,コンピュータビジョン入門・ロボットビジョン入門として,レガシー前処理の学習も欠かせません.

「色」をトリガーにして画像処理を行うことは基本です.
例えば,ロボカップ・サッカーでは,昔は「オレンジ色のボール」「青色・黄色のゴール」「ロボットは黒色ベースでシアン・マゼンダのゼッケン」など色の制約を設けて,画像認識させていました.
参加者も審判も,そして観客も,派手な色物の服装は禁止されていました(^_^

また,ラベリング処理はOpenCV 3.0から標準関数として実装されましたが,それ以前はフルスクラッチで書く必要がありました.フルスクラッチだとプログラマーの技量によって処理速度が変わるので,腕の見せ所でした.

今回はOpenCVのラベリング関数を使って,色を基準として物体を識別しTelloで追跡させてみます.

具体的には,この動画の様に,オレンジ色のカラーコーンを追いかけさせます.


カラーコーンを追いかけて,左右旋回をさせるだけの簡単なプログラムですが,バックグラウンドには多くの要素技術があるので,記事が長めになっています.

前提条件

ホームフォルダにTello-Pythonがインストールされているという前提で話を進めます.

Linuxマシンであれば /home/(ユーザー名)/ に,Tello-Pythonというフォルダがあることになります.

詳しくは Tello-Pythonのダウンロード を御覧ください.

今回の作業内容

合計で5種類のプログラムを紹介します.

  1. TelloのRGB画像とHSV画像を見るだけのサンプルプログラム
  2. TelloのRGB画像で二値化するサンプルプログラム
  3. TelloのHSV画像で二値化するサンプルプログラム
  4. TelloのHSV画像で二値化・ラベリング・面積&重心計算するプログラム
  5. 物体を検出してTelloが追いかけるプログラム

作業ディレクトリの作成

まずは,Tello-CV-coreをコピーして,新しいプロジェクト(ディレクトリ)Tello-CV-colorを作ります.

Tello-CV-coreをディレクトリごとコピー
$ cd ~/Tello-Python/
$ cp -R Tello-CV-core Tello-CV-color
$ cd Tello-CV-color

tello.pyとlibh264decoder.soのコピーの手間など考えると,フォルダごとコピーが一番楽ですね.

(1)RGB画像とHSV画像を見るだけのサンプルプログラム

Telloの画像はのRGB3色ですが,色をキーにして物体検出をさせる時は,色相(Hue)・彩度(Saturation)・明度(Value)のHSV色空間を使ったほうが検知し易いです.

  参考:【python/OpenCV】画像の特定の色を抽出する方法
     色空間の変換

この章では,まずはRGB画像とHSV画像を見てもらいます.
とは言っても,本当のHSV画像は色空間が異なるだけでRGB画像と全く同じものです.
そうではなく,HSVのデータを無理やりRGBデータとして表示させて見るという荒業です(^^

main_color.py

プログラムはmain.pyに書き加える形で作成しました.別名で保存しています.

書き加えの手間を省くなら,以下のコードをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください.

main_colors.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため

# メイン関数
def main():
    # Telloクラスを使って,droneというインスタンス(実体)を作る
    drone = tello.Tello('', 8889, command_timeout=.01)  

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ


    #Ctrl+cが押されるまでループ
    try:
        while True:
            # (A)画像取得
            frame = drone.read()    # 映像を1フレーム取得
            if frame is None or frame.size == 0:    # 中身がおかしかったら無視
                continue 

            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            bgr_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更

            hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)  # BGR画像 -> HSV画像

            # (X)ウィンドウに表示
            cv2.imshow('BGR Color', bgr_image)  # 2つのウィンドウを作る
            cv2.imshow('HSV Color', hsv_image)

            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出,プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                drone.land()                # 着陸
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降

            # (Z)5秒おきに'command'を送って、死活チェックを通す
            current_time = time.time()  # 現在時刻を取得
            if current_time - pre_time > 5.0 :  # 前回時刻から5秒以上経過しているか?
                drone.send_command('command')   # 'command'送信
                pre_time = current_time         # 前回時刻を更新

    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

    # telloクラスを削除
    del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":      # importされると"__main__"は入らないので,実行かimportかを判断できる.
    main()    # メイン関数を実行

プログラム解説

Tello-CV-coreスケルトンと異なる点は,永久ループ内の(B),(X)ブロックのみです.

スケルトンからの変更箇所
            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            bgr_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更

            hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)  # BGR画像 -> HSV画像

            # (X)ウィンドウに表示
            cv2.imshow('BGR Color', bgr_image)  # 2つのウィンドウを作る
            cv2.imshow('HSV Color', hsv_image)

OpenCVで取り扱う画像の色の並び方は,青・緑・赤の順なので「BGR画像」と呼ばれます.RGB画像とは逆の並びです.

cv2.cvtColor関数を使ってBGR画像をHSV画像に変換し,それぞれの画像を別々のウィンドウに表示させています.

プログラム実行

プログラム本体はmain_color.pyです.

プログラムの実行
$ python main_color.py

今までと同様にctrl+cを押すことで,プログラムを終了することもできますが,
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう.

実行結果

下図の様に,2枚の画像が表示されれば成功です.
bgr_hsv.png
元画像であるBGRのウィンドウと,HSVを無理やりBGR化して表示したウィンドウが表示されています.

「RGBとは異なる色空間」というものはなかなか理解し難いのですが,このように無理やり表示させることで,「昆虫か何かの別の動物にはこんな風に見えている」とでも思えば,まだなんとか理解できるのではないでしょうか.

(2)TelloのRGB画像で二値化するサンプルプログラム

次はRGB画像(厳密にはBGR画像)を二値化してみます.

本来,二値化というのは,あるしきい値(閾値)を境目にして,それ未満の画素値だったらゼロ・それ以上の画素値だったら255,の2つの値にする処理のことです.cv2.threshold関数を使うことで実現できます.
  参考:OpenCV 画像の二値化

しかし「境界線が1本で,その上か下かだけで判断する」という処理で,目的の物体を検出するのは大変です.

そこで「最小しきい値と,最大しきい値の2つを設け,その範囲内にある色を255とする」という処理がよく使われます.それがcv2.inRange関数です.

  参考OpenCV - inRange による範囲指定で2値化する方法について

この章では,cv2.inRange関数を使って,Telloに写った画像を二値化処理してみます.

main_bgr.py

プログラムはmain.pyに書き加える形で作成しました.別名で保存しています.

書き加えの手間を省くなら,以下のコードをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください.

main_bgr.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため

# メイン関数
def main():
    # Telloクラスを使って,droneというインスタンス(実体)を作る
    drone = tello.Tello('', 8889, command_timeout=.01)  

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ

    # トラックバーを作るため,まず最初にウィンドウを生成
    cv2.namedWindow("OpenCV Window")

    # トラックバーのコールバック関数は何もしない空の関数
    def nothing(x):
        pass        # passは何もしないという命令

    # トラックバーの生成
    cv2.createTrackbar("R_min", "OpenCV Window", 0, 255, nothing)
    cv2.createTrackbar("R_max", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("G_min", "OpenCV Window", 0, 255, nothing)
    cv2.createTrackbar("G_max", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("B_min", "OpenCV Window", 0, 255, nothing)
    cv2.createTrackbar("B_max", "OpenCV Window", 128, 255, nothing)


    #Ctrl+cが押されるまでループ
    try:
        while True:

            # (A)画像取得
            frame = drone.read()    # 映像を1フレーム取得
            if frame is None or frame.size == 0:    # 中身がおかしかったら無視
                continue 

            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # RGB並びをOpenCV用のBGR並びに変換する
            bgr_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更

            # トラックバーの値を取る
            r_min = cv2.getTrackbarPos("R_min", "OpenCV Window")
            r_max = cv2.getTrackbarPos("R_max", "OpenCV Window")
            g_min = cv2.getTrackbarPos("G_min", "OpenCV Window")
            g_max = cv2.getTrackbarPos("G_max", "OpenCV Window")
            b_min = cv2.getTrackbarPos("B_min", "OpenCV Window")
            b_max = cv2.getTrackbarPos("B_max", "OpenCV Window")

            # inRange関数で範囲指定2値化 -> マスク画像として使う
            mask_image = cv2.inRange(bgr_image, (b_min, g_min, r_min), (b_max, g_max, r_max)) # BGR画像なのでタプルもBGR並び

            # bitwise_andで元画像にマスクをかける -> マスクされた部分の色だけ残る
            result_image = cv2.bitwise_and(bgr_image, bgr_image, mask=mask_image)   # BGR画像 AND BGR画像 なので,自分自身とのANDは何も変化しない ->マスクだけ効かせる

            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', result_image)

            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出,プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                drone.land()                # 着陸
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降

            # (Z)5秒おきに'command'を送って、死活チェックを通す
            current_time = time.time()  # 現在時刻を取得
            if current_time - pre_time > 5.0 :  # 前回時刻から5秒以上経過しているか?
                drone.send_command('command')   # 'command'送信
                pre_time = current_time         # 前回時刻を更新

    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

    # telloクラスを削除
    del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":      # importされると"__main__"は入らないので,実行かimportかを判断できる.
    main()    # メイン関数を実行

cv2.inRange関数の理解のために,cv2.imshowで作ったウィンドウにトラックバーを表示させています.

簡単にプログラムを解説すると,
プログラムの初期化部では,

・ウィンドウの作成
・トラックバー用のコールバック関数として引き渡すダミー関数の作成
・トラックバーの作成

を行い,
ループ部では

・トラックバーの位置を取得
cv2.inRangeで二値化
cv2.bitwise_andで,二値化で255になった画像をマスクに使い,元画像の色を抽出

の結果を表示させています.

プログラム実行

プログラム本体はmain_bgr.pyです.

プログラムの実行
$ python main_bgr.py

今までと同様にctrl+cを押すことで,プログラムを終了することもできますが,
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう.

実行結果

下図の様なウィンドウが表示されれば成功です.
tello_color_bgr.png
ある特定の色だけが抜け落ちた画像が表示されます.

プログラム実行直後は,

  • Rの範囲0〜128
  • Gの範囲0〜128
  • Bの範囲0〜128

この3つの条件に合致する画素値だけが取り出されています.
そのため(R,G,B)=(255,255,255)すなわち白色に近い方の色が抜け落ちています.

使い方

ウィンドウの下に追加されているトラックバー(スライダー)を操作して,色の範囲を変えてみましょう.

例えば,

  • Rの範囲0〜255
  • Gの範囲0〜255
  • Bの範囲0〜255

として全部を指定すれば,元画像のままです.
tello_color_sample.png

それを,

  • Rの範囲160〜210
  • Gの範囲98〜152
  • Bの範囲112〜164

に絞り込んでみると,この様にピンク色だけが取り出せました.
tello_color_sample_pink.png

同様に,

  • Rの範囲89〜109
  • Gの範囲123〜191
  • Bの範囲82〜117

黃緑色だけ取り出すこともできます.
tello_color_sample_green.png

この様に,自分が狙った色だけを抽出する際の最小値・最大値を決めるために,このプログラムを使います.

(3)TelloのHSV画像で二値化するサンプルプログラム

この章ではHSV空間で二値化をしてみます.

HSVへの変換や,cv2.inRangeによる範囲指定は既にやりましたから,少し変更するだけで対応が可能です.

main_hsv.py

プログラムはmain.pyに書き加える形で作成しました.別名で保存しています.

書き加えの手間を省くなら,以下のコードをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください.

main_hsv.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため

# メイン関数
def main():
    # Telloクラスを使って,droneというインスタンス(実体)を作る
    drone = tello.Tello('', 8889, command_timeout=.01)  

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ

    # トラックバーを作るため,まず最初にウィンドウを生成
    cv2.namedWindow("OpenCV Window")

    # トラックバーのコールバック関数は何もしない空の関数
    def nothing(x):
        pass

    # トラックバーの生成
    cv2.createTrackbar("H_min", "OpenCV Window", 0, 179, nothing)       # Hueの最大値は179
    cv2.createTrackbar("H_max", "OpenCV Window", 128, 179, nothing)
    cv2.createTrackbar("S_min", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("S_max", "OpenCV Window", 255, 255, nothing)
    cv2.createTrackbar("V_min", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("V_max", "OpenCV Window", 255, 255, nothing)

    #Ctrl+cが押されるまでループ
    try:
        while True:

            # (A)画像取得
            frame = drone.read()    # 映像を1フレーム取得
            if frame is None or frame.size == 0:    # 中身がおかしかったら無視
                continue 

            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            bgr_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更

            hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)  # BGR画像 -> HSV画像

            # トラックバーの値を取る
            h_min = cv2.getTrackbarPos("H_min", "OpenCV Window")
            h_max = cv2.getTrackbarPos("H_max", "OpenCV Window")
            s_min = cv2.getTrackbarPos("S_min", "OpenCV Window")
            s_max = cv2.getTrackbarPos("S_max", "OpenCV Window")
            v_min = cv2.getTrackbarPos("V_min", "OpenCV Window")
            v_max = cv2.getTrackbarPos("V_max", "OpenCV Window")

            # inRange関数で範囲指定2値化 -> マスク画像として使う
            mask_image = cv2.inRange(hsv_image, (h_min, s_min, v_min), (h_max, s_max, v_max)) # HSV画像なのでタプルもHSV並び

            # bitwise_andで元画像にマスクをかける -> マスクされた部分の色だけ残る
            result_image = cv2.bitwise_and(hsv_image, hsv_image, mask=mask_image)

            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', result_image)   # ウィンドウに表示するイメージを変えれば色々表示できる

            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出,プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                drone.land()                # 着陸
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降

            # (Z)5秒おきに'command'を送って、死活チェックを通す
            current_time = time.time()  # 現在時刻を取得
            if current_time - pre_time > 5.0 :  # 前回時刻から5秒以上経過しているか?
                drone.send_command('command')   # 'command'送信
                pre_time = current_time         # 前回時刻を更新

    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

    # telloクラスを削除
    del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":      # importされると"__main__"は入らないので,実行かimportかを判断できる.
    main()    # メイン関数を実行

RGB用だったトラックバーを,色相(Hue)・彩度(Saturation)・明度(Value)用に変更しただけです.ただし,Hueは0〜179までの値しか取りません.

プログラム実行

プログラム本体はmain_hsv.pyです.

プログラムの実行
$ python main_hsv.py

今までと同様にctrl+cを押すことで,プログラムを終了することもできますが,
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう.

実行結果

例えば,

  • Hueの範囲0〜179
  • Saturationの範囲0〜255
  • Valueの範囲0〜255

の全ての範囲で下図の様に見える画像も,
tello_color_sample_hsv.png

  • Hueの範囲161〜179
  • Saturationの範囲81〜130
  • Valueの範囲151〜193

と絞り込んでやると,この様になります.
tello_color_sample_hsv_pink.png

やっている事はRGBの時と大差ありませんが,HSV色空間を使うと目的の色を指定しやすいのが特徴です.

というのは,Hueは色相ですから,色相環をぐるっと一周回れば全ての色が表現できるわけです.つまり,Hueだけで色がほぼ特定できることになります.
  参考:色相
余談ですが,本来の色相は0〜360で一周ですが,OpenCVでは半分にして表現しています.255以上の数値は8ビットでは足りなくなってしまうので.

(4)TelloのHSV画像で二値化・ラベリング・面積&重心計算するプログラム

目的の物体をHSV空間で絞り込むことができたら,ラベリングで物体の個数を数えます.

参考
  Python+OpenCVでラベリング
  OpenCV - connectedComponents() で連結成分のラベリング
  Python+OpenCVを利用したラベリング処理
  [OpenCV Python]OpenCVを使ったラベリング
  【Python/OpenCV】最大面積のブロブ解析(座標・大きさなど)

具体的には,OpenCV3から導入されたcv2.connectedComponentsWithStats関数を使います.
同じ値を持つ隣り合った画素の塊を数えるので,「connectedComponents=連結コンポーネント」と言う呼び方になっていますが,少なくとも日本では「ラベリング」と言ったほうが通じます.

main_label.py

プログラムはmain.pyを書き加える形で作成しました.

書き加えの手間を省くなら,以下のコードをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください.

main_label.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため
import numpy as np

# メイン関数
def main():
    # Telloクラスを使って,droneというインスタンス(実体)を作る
    drone = tello.Tello('', 8889, command_timeout=.01)  

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ

    # トラックバーを作るため,まず最初にウィンドウを生成
    cv2.namedWindow("OpenCV Window")

    # トラックバーのコールバック関数は何もしない空の関数
    def nothing(x):
        pass

    # トラックバーの生成
    cv2.createTrackbar("H_min", "OpenCV Window", 0, 179, nothing)
    cv2.createTrackbar("H_max", "OpenCV Window", 9, 179, nothing)       # Hueの最大値は179
    cv2.createTrackbar("S_min", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("S_max", "OpenCV Window", 255, 255, nothing)
    cv2.createTrackbar("V_min", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("V_max", "OpenCV Window", 255, 255, nothing)

    #Ctrl+cが押されるまでループ
    try:
        while True:

            # (A)画像取得
            frame = drone.read()    # 映像を1フレーム取得
            if frame is None or frame.size == 0:    # 中身がおかしかったら無視
                continue 

            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            bgr_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更

            hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)  # BGR画像 -> HSV画像

            # トラックバーの値を取る
            h_min = cv2.getTrackbarPos("H_min", "OpenCV Window")
            h_max = cv2.getTrackbarPos("H_max", "OpenCV Window")
            s_min = cv2.getTrackbarPos("S_min", "OpenCV Window")
            s_max = cv2.getTrackbarPos("S_max", "OpenCV Window")
            v_min = cv2.getTrackbarPos("V_min", "OpenCV Window")
            v_max = cv2.getTrackbarPos("V_max", "OpenCV Window")

            # inRange関数で範囲指定2値化
            bin_image = cv2.inRange(hsv_image, (h_min, s_min, v_min), (h_max, s_max, v_max)) # HSV画像なのでタプルもHSV並び

            # bitwise_andで元画像にマスクをかける -> マスクされた部分の色だけ残る
            masked_image = cv2.bitwise_and(hsv_image, hsv_image, mask=bin_image)

            # ラベリング結果書き出し用に画像を準備
            out_image = masked_image

            # 面積・重心計算付きのラベリング処理を行う
            num_labels, label_image, stats, center = cv2.connectedComponentsWithStats(bin_image)

            # 最大のラベルは画面全体を覆う黒なので不要.データを削除
            num_labels = num_labels - 1
            stats = np.delete(stats, 0, 0)
            center = np.delete(center, 0, 0)

            # 検出したラベルの数だけ繰り返す
            for index in range(num_labels):
                # ラベルのx,y,w,h,面積s,重心位置mx,myを取り出す
                x = stats[index][0]
                y = stats[index][1]
                w = stats[index][2]
                h = stats[index][3]
                s = stats[index][4]
                mx = int(center[index][0])
                my = int(center[index][1])
                #print("(x,y)=%d,%d (w,h)=%d,%d s=%d (mx,my)=%d,%d"%(x, y, w, h, s, mx, my) )

                # ラベルを囲うバウンディングボックスを描画
                cv2.rectangle(out_image, (x, y), (x+w, y+h), (255, 0, 255))

                # 重心位置の座標と面積を表示
                cv2.putText(out_image, "%d,%d"%(mx,my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))
                cv2.putText(out_image, "%d"%(s), (x, y+h+30), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))

            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', out_image)  # ウィンドウに表示するイメージを変えれば色々表示できる

            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出,プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                drone.land()                # 着陸
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降

            # (Z)5秒おきに'command'を送って、死活チェックを通す
            current_time = time.time()  # 現在時刻を取得
            if current_time - pre_time > 5.0 :  # 前回時刻から5秒以上経過しているか?
                drone.send_command('command')   # 'command'送信
                pre_time = current_time         # 前回時刻を更新

    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

    drone.send_command('streamoff')
    # telloクラスを削除
    del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":      # importされると"__main__"は入らないので,実行かimportかを判断できる.
    main()    # メイン関数を実行

プログラム解説

プログラムはHSV二値化とほぼ同じですが,ラベリング処理と処理結果表示を追加しました.

ラベリング処理
            # inRange関数で範囲指定2値化
            bin_image = cv2.inRange(hsv_image, (h_min, s_min, v_min), (h_max, s_max, v_max)) # HSV画像なのでタプルもHSV並び

            # bitwise_andで元画像にマスクをかける -> マスクされた部分の色だけ残る
            masked_image = cv2.bitwise_and(hsv_image, hsv_image, mask=bin_image)

            # ラベリング結果書き出し用に画像を準備
            out_image = masked_image

            # 面積・重心計算付きのラベリング処理を行う
            num_labels, label_image, stats, center = cv2.connectedComponentsWithStats(bin_image)

            # 最大のラベルは画面全体を覆う黒なので不要.データを削除
            num_labels = num_labels - 1  # 黒背景のぶん1減らす
            stats = np.delete(stats, 0, 0)   # 黒背景の結果を消して,自分自身に置き換える
            center = np.delete(center, 0, 0)

ラベリングcv2.connectedComponentsWithStats関数には,cv2.inRangeが出力した二値画像(今回の変数名はbin_image)を入れます.
masked_imageやout_imageは,あくまで人間が結果を見るためのイメージに過ぎません.大事なのはbin_imageの方です.

非常に重要な点はcv2.connectedComponentsWithStats関数は,黒で塗りつぶされた背景色全体も1つのラベルとして認識することです.
一般的なラベリングでは,値が0の画素は無視します.しかし,OpenCVのこの関数では1個目かつ最大面積のラベルは「背景全体」になるのです.(小さな親切・大きなお世話な機能)

この「背景全体」の情報は不要です.したがって,検出したラベル数を1つ減らし,stats,centerなどのリストから1番目の要素を削除する処理が絶対必要です.
  参考:NumPyで任意の行・列を削除するdeleteの使い方

statscenterの配列の中身は,以下のようになっています.

ラベリング結果の配列の中身
stats = [
         [ x  y  w  h  s]   # index番号0  黒背景のラベル情報
         [ x  y  w  h  s]   # index番号1
         [ x  y  w  h  s]   #    〃    2
          ...
        ]
center =[
         [ mx  my ]   # index番号0  黒背景のラベル情報
         [ mx  my ]   # index番号1
         [ mx  my ]   #     〃   2
          ...
        ]

したがって,最初の黒背景の行を全部削除するには,
stats = np.delete(stats, 0, 0) obj=0,axis=0で削除
center = np.delete(center, 0, 0)
と書いて削除した結果を自分自身に代入し直しています.

この削除作業を忘れると,「一番大きな(=最大面積を持つ)ラベルを探す」という処理をすると,絶対に背景ラベルが1番になるという罠に陥ります(^^;;

次は,人間が見るためのイメージ(out_image)に,四角を描いたり,重心位置や面積を書き込みます.
検出したラベルの個数に合わせて,forループを回しています.

ラベリング処理の結果を元に,枠線を描いたり,テキストを描いたりする部分
            # 検出したラベルの数だけ繰り返す
            for index in range(num_labels):
                # ラベルのx,y,w,h,面積s,重心位置mx,myを取り出す
                x = stats[index][0]
                y = stats[index][1]
                w = stats[index][2]
                h = stats[index][3]
                s = stats[index][4]
                mx = int(center[index][0])
                my = int(center[index][1])
                #print("(x,y)=%d,%d (w,h)=%d,%d s=%d (mx,my)=%d,%d"%(x, y, w, h, s, mx, my) )

                # ラベルを囲うバウンディングボックスを描画
                cv2.rectangle(out_image, (x, y), (x+w, y+h), (255, 0, 255))

                # 重心位置の座標と面積を表示
                cv2.putText(out_image, "%d,%d"%(mx,my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))
                cv2.putText(out_image, "%d"%(s), (x, y+h+30), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))

興味がある人は,print文のコメントを外して,テキスト表示も見てみると良いでしょう.

プログラム実行

プログラムはmain_label.pyです.

プログラムの実行
$ python main_label.py

今までと同様にctrl+cを押すことで,プログラムを終了することもできますが,
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう.

実行結果

tello_label_sample.png
上図の様に,複数のカラーコーンをTelloに見せた際の処理結果は,こうなります.
tello_label_result.png

検出したオブジェクト(ラベル)の重心位置と面積が表示されています.

また,カラーコーンのエッジ部分に現れた細かいゴミ(面積が1〜5程度)も検出してしまっています.本来このようなゴミは,膨張・収縮・孤立点除去などの前処理で落とすべきですね.

(5)物体を検出してTelloが追いかけるプログラム

main_control.py

プログラム本体であるmain_control.pyは,スケルトンプログラムに書き加える形で作成しました.

書き加えの手間を省くなら,以下のコードをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください.

main_control.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため
import numpy as np

# メイン関数
def main():
    # Telloクラスを使って,droneというインスタンス(実体)を作る
    drone = tello.Tello('', 8889, command_timeout=.01)  

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ

    # トラックバーを作るため,まず最初にウィンドウを生成
    cv2.namedWindow("OpenCV Window")

    # トラックバーのコールバック関数は何もしない空の関数
    def nothing(x):
        pass

    # トラックバーの生成
    cv2.createTrackbar("H_min", "OpenCV Window", 0, 179, nothing)
    cv2.createTrackbar("H_max", "OpenCV Window", 9, 179, nothing)       # Hueの最大値は179
    cv2.createTrackbar("S_min", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("S_max", "OpenCV Window", 255, 255, nothing)
    cv2.createTrackbar("V_min", "OpenCV Window", 128, 255, nothing)
    cv2.createTrackbar("V_max", "OpenCV Window", 255, 255, nothing)

    flag = 0
    #Ctrl+cが押されるまでループ
    try:
        while True:

            # (A)画像取得
            frame = drone.read()    # 映像を1フレーム取得
            if frame is None or frame.size == 0:    # 中身がおかしかったら無視
                continue 

            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            bgr_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更

            hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)  # BGR画像 -> HSV画像

            # トラックバーの値を取る
            h_min = cv2.getTrackbarPos("H_min", "OpenCV Window")
            h_max = cv2.getTrackbarPos("H_max", "OpenCV Window")
            s_min = cv2.getTrackbarPos("S_min", "OpenCV Window")
            s_max = cv2.getTrackbarPos("S_max", "OpenCV Window")
            v_min = cv2.getTrackbarPos("V_min", "OpenCV Window")
            v_max = cv2.getTrackbarPos("V_max", "OpenCV Window")

            # inRange関数で範囲指定2値化
            bin_image = cv2.inRange(hsv_image, (h_min, s_min, v_min), (h_max, s_max, v_max)) # HSV画像なのでタプルもHSV並び

            # bitwise_andで元画像にマスクをかける -> マスクされた部分の色だけ残る
            masked_image = cv2.bitwise_and(hsv_image, hsv_image, mask=bin_image)

            # ラベリング結果書き出し用に画像を準備
            out_image = masked_image

            # 面積・重心計算付きのラベリング処理を行う
            num_labels, label_image, stats, center = cv2.connectedComponentsWithStats(bin_image)

            # 最大のラベルは画面全体を覆う黒なので不要.データを削除
            num_labels = num_labels - 1
            stats = np.delete(stats, 0, 0)
            center = np.delete(center, 0, 0)


            if num_labels >= 1:
                # 面積最大のインデックスを取得
                max_index = np.argmax(stats[:,4])
                #print max_index

                # 面積最大のラベルのx,y,w,h,面積s,重心位置mx,myを得る
                x = stats[max_index][0]
                y = stats[max_index][1]
                w = stats[max_index][2]
                h = stats[max_index][3]
                s = stats[max_index][4]
                mx = int(center[max_index][0])
                my = int(center[max_index][1])
                #print("(x,y)=%d,%d (w,h)=%d,%d s=%d (mx,my)=%d,%d"%(x, y, w, h, s, mx, my) )

                # ラベルを囲うバウンディングボックスを描画
                cv2.rectangle(out_image, (x, y), (x+w, y+h), (255, 0, 255))

                # 重心位置の座標を表示
                #cv2.putText(out_image, "%d,%d"%(mx,my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))
                cv2.putText(out_image, "%d"%(s), (x, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))

                if flag == 1:
                    a = b = c = d = 0

          # P制御の式(Kpゲインはとりあえず1.0)
                    dx = 1.0 * (240 - mx)       # 画面中心との差分

                    # 旋回方向の不感帯を設定
                    d = 0.0 if abs(dx) < 50.0 else dx   # ±50未満ならゼロにする

                    d = -d
                    # 旋回方向のソフトウェアリミッタ(±100を超えないように)
                    d =  100 if d >  100.0 else d
                    d = -100 if d < -100.0 else d

                    print('dx=%f'%(dx) )
                    drone.send_command('rc %s %s %s %s'%(int(a), int(b), int(c), int(d)) )


            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', out_image)  # ウィンドウに表示するイメージを変えれば色々表示できる

            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出,プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                drone.land()                # 着陸
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降
            elif key == ord('1'):
                flag = 1                    # 追跡モードON
            elif key == ord('2'):
                flag = 0                    # 追跡モードOFF

            # (Z)5秒おきに'command'を送って、死活チェックを通す
            current_time = time.time()  # 現在時刻を取得
            if current_time - pre_time > 5.0 :  # 前回時刻から5秒以上経過しているか?
                drone.send_command('command')   # 'command'送信
                pre_time = current_time         # 前回時刻を更新

    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

    drone.send_command('streamoff')
    # telloクラスを削除
    del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":      # importされると"__main__"は入らないので,実行かimportかを判断できる.
    main()    # メイン関数を実行

プログラム解説

前章のラベリング処理では,検出した複数ラベル全てを表示して四角で囲い,座標や面積を表示していました.しかし今回は,最も大きな(面積最大の)ラベルだけを追いかけるので,for文は使いません.

ラベル情報を持つ配列statsには,以下の様にデータが入っています.

4列目を縦に比較して,最大面積を持つsを探したい
stats = [ #0  1  2  3  4 列目 
         [ x  y  w  h  s]   # index番号0  黒背景のラベル情報は既に削除済み
         [ x  y  w  h  s]   # index番号1
         [ x  y  w  h  s]   #    〃    2
          ...
        ]

最大の面積を持つインデックスを探したいので,NumPyのargmax関数を使って探します.
4列目に対してargmaxをかけたいので,書式はmax_index = np.argmax(stats[:,4])となるわけです.
  参考:[Python]Numpyの参照、抽出、結合/列を抽出する

こうして書いたプログラムが,以下になります.

最大面積を持つラベルを抽出する
            if num_labels >= 1:  # ラベル数が0の(何も見えない)時は何もしない
                # 面積最大のインデックスを取得
                max_index = np.argmax(stats[:,4])
                #print max_index

                # 面積最大のラベルのx,y,w,h,面積s,重心位置mx,myを得る
                x = stats[max_index][0]
                y = stats[max_index][1]
                w = stats[max_index][2]
                h = stats[max_index][3]
                s = stats[max_index][4]
                mx = int(center[max_index][0])
                my = int(center[max_index][1])
                #print("(x,y)=%d,%d (w,h)=%d,%d s=%d (mx,my)=%d,%d"%(x, y, w, h, s, mx, my) )

                # ラベルを囲うバウンディングボックスを描画
                cv2.rectangle(out_image, (x, y), (x+w, y+h), (255, 0, 255))

                # 重心位置の重心と面積を表示
                #cv2.putText(out_image, "%d,%d"%(mx,my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))
                cv2.putText(out_image, "%d"%(s), (x, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))

最大面積を持つ要素番号max_indexを求め,その行のx,y,w,h,s,mx,myを取り出しています.
ついでに,四角い枠と面積を描いています.

続いては,Telloの制御を行うプログラムです.

自動制御フラグが1の時だけ,Telloを動かす
                if flag == 1:
                    a = b = c = d = 0

          # P制御の式(Kpゲインはとりあえず1.0)
                    dx = 1.0 * (240 - mx)       # 画面中心との差分

                    # 旋回方向の不感帯を設定
                    d = 0.0 if abs(dx) < 50.0 else dx   # ±50未満ならゼロにする

                    d = -d
                    # 旋回方向のソフトウェアリミッタ(±100を超えないように)
                    d =  100 if d >  100.0 else d
                    d = -100 if d < -100.0 else d

                    print('dx=%f'%(dx) )
                    drone.send_command('rc %s %s %s %s'%(int(a), int(b), int(c), int(d)) )

まずはflagをチェックして,Telloの制御のOn/Offを確認しています.Offの時は何もしません.なお,このflagはキーボード入力12で変更できるようになっています.

a,b,c,dはrcコマンドで送信するスティック入力量を格納する変数です.デフォルトでは動かないのでゼロにしておきます.

今回は左右に旋回するだけなので,画面中心(240,180)のX軸方向240にだけ注目します.

  • x軸方向の偏差(240-mx)にPゲイン1.0を掛けたものをdx

として,制御式を作っています.
ゲインが1.0だと,かなりピーキーです.Telloが左右に振動してしまう場合は,0.1ずつ減らして調整しましょう.
追跡したい対象物は,Telloから1メートル以上離れた状態で実験してください.

今回の制御で使ったのは,もっとも基本的なP制御です.(PID制御のP)

また制御式で得られたdxに,不感帯とソフトウェアリミッタの処理をかけています.
三項演算子を使った不感帯・ソフトリミッタ処理についての詳しくは, こちら で説明しています.

プログラムの実行

プログラム本体はmain_control.pyです.

プログラムの実行
$ python main_control.py

今までと同様にctrl+cを押すことで,プログラムを終了することもできますが,
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう.

操作系

操作系は以下の様になっています.
1キーで色追従のフィードバック制御がON(有効)になり,
2キーでOFF(無効)になります.
cv_face_key.png

フィードバック制御がONになると,Telloは左右旋回だけを行います.

操作手順

  1. tキーで離陸させる.
  2. 上下前後左右の移動キーで,顔が連続して認識できる位置(安全な位置)までTelloを手動操作する.
  3. 1キーを押してフィードバック制御を開始させる.
  4. カラーコーンを左右に動かして,Telloが追従してくる事を確認する.
  5. もしTelloが意図しない方向へ流れ始め(暴走し)たら,2キーを押して制御を終了させ,移動キーでTelloを止める.
  6. lキーで着陸させる.

Tello SDKのrcコマンドを使って操作しているので,機体が流れ始めた際に止めるのは手動操作だけです.(移動コマンドは応答が遅いので使っていません)

実行結果

以下の様に最大面積を持つラベルだけが表示されます.
tello_label_control.png

離陸して,追跡させる物体を検出していることを確認したら,1キーを押して自動制御させます.
うまくいけば,冒頭でも紹介したこの動画の様にTelloが向きを変えます.

おわりに

今回は,OpenCVを使って二値化・ラベリング・面積/重心計算という,スタンダードな画像処理を試しました.

色の範囲を指定して二値化する手法は,周囲の明るさが大きく影響します.これを「環境条件に依存」「照明条件に依存」と呼びます.部屋の明るさが変わったり,屋外だと雲で日光が遮られたりすると,途端に検出できなくなることも多いです.

環境変化にロバスト(頑健な)画像処理」というのは,ロボットのプログラマー皆の目標です.

Why do not you register as a user and use Qiita more conveniently?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away