はじめに
このページは，
の１ページです．
全体を見たい場合は上記ページへお戻りください．
概要
画像処理で物体検出，といえば「二値化・ラベリング・面積＆重心計算」が古くから定番です．
最近はそんなレガシー(伝統的・遺産的)な前処理なんか省いて，ディープ・ラーニングで検出してしまう「Semantic Segmentation」の方が流行しています．性能もすごいし．．．
参考(動画を再生)：http://mprg.jp/research/segmentation_e
とはいえ，コンピュータビジョン入門・ロボットビジョン入門として，レガシー前処理の学習も欠かせません．
「色」をトリガーにして画像処理を行うことは基本です．
例えば，ロボカップ・サッカーでは，昔は「オレンジ色のボール」「青色・黄色のゴール」「ロボットは黒色ベースでシアン・マゼンダのゼッケン」など色の制約を設けて，画像認識させていました．
参加者も審判も，そして観客も，派手な色物の服装は禁止されていました(^_^
また，ラベリング処理はOpenCV 3.0から標準関数として実装されましたが，それ以前はフルスクラッチで書く必要がありました．フルスクラッチだとプログラマーの技量によって処理速度が変わるので，腕の見せ所でした．
今回はOpenCVのラベリング関数を使って，色を基準として物体を識別しTelloで追跡させてみます．
具体的には，この動画の様に，オレンジ色のカラーコーンを追いかけさせます．
オレンジ色を２値化・ラベリング・重心計算してトラッキング。#tello pic.twitter.com/lx6aNiUEVg
— hsgucci404 (@hsgucci404) October 16, 2019
カラーコーンを追いかけて，左右旋回をさせるだけの簡単なプログラムですが，バックグラウンドには多くの要素技術があるので，記事が長めになっています．
前提条件
ホームフォルダにTello-Pythonがインストールされているという前提で話を進めます．
Linuxマシンであれば /home/(ユーザー名)/ に，Tello-Pythonというフォルダがあることになります．
詳しくは Tello-Pythonのダウンロード を御覧ください．
今回の作業内容
合計で５種類のプログラムを紹介します．
- TelloのRGB画像とHSV画像を見るだけのサンプルプログラム
- TelloのRGB画像で二値化するサンプルプログラム
- TelloのHSV画像で二値化するサンプルプログラム
- TelloのHSV画像で二値化・ラベリング・面積＆重心計算するプログラム
- 物体を検出してTelloが追いかけるプログラム
作業ディレクトリの作成
まずは，Tello-CV-coreをコピーして，新しいプロジェクト(ディレクトリ)Tello-CV-colorを作ります．
$ cd ~/Tello-Python/
$ cp -R Tello-CV-core Tello-CV-color
$ cd Tello-CV-color
tello.pyとlibh264decoder.soのコピーの手間など考えると，フォルダごとコピーが一番楽ですね．
(1)RGB画像とHSV画像を見るだけのサンプルプログラム
Telloの画像は赤緑青のRGB３色ですが，色をキーにして物体検出をさせる時は，色相(Hue)・彩度(Saturation)・明度(Value)のHSV色空間を使ったほうが検知し易いです．
参考：【python/OpenCV】画像の特定の色を抽出する方法
色空間の変換
この章では，まずはRGB画像とHSV画像を見てもらいます．
とは言っても，本当のHSV画像は色空間が異なるだけでRGB画像と全く同じものです．
そうではなく，HSVのデータを無理やりRGBデータとして表示させて見るという荒業です(^^
main_color.py
プログラムはmain.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キーを押して終了するのが良いでしょう．
実行結果
下図の様に，２枚の画像が表示されれば成功です．
元画像であるBGRのウィンドウと，HSVを無理やりBGR化して表示したウィンドウが表示されています．
「RGBとは異なる色空間」というものはなかなか理解し難いのですが，このように無理やり表示させることで，「昆虫か何かの別の動物にはこんな風に見えている」とでも思えば，まだなんとか理解できるのではないでしょうか．
(2)TelloのRGB画像で二値化するサンプルプログラム
次はRGB画像(厳密にはBGR画像)を二値化してみます．
本来，二値化というのは，あるしきい値(閾値)を境目にして，それ未満の画素値だったらゼロ・それ以上の画素値だったら255，の２つの値にする処理のことです．cv2.threshold関数を使うことで実現できます．
参考：OpenCV 画像の二値化
しかし「境界線が１本で，その上か下かだけで判断する」という処理で，目的の物体を検出するのは大変です．
そこで「最小しきい値と，最大しきい値の２つを設け，その範囲内にある色を255とする」という処理がよく使われます．それがcv2.inRange関数です．
参考OpenCV - inRange による範囲指定で2値化する方法について
この章では，cv2.inRange関数を使って，Telloに写った画像を二値化処理してみます．
main_bgr.py
プログラムはmain.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関数で範囲指定２値化 -> マスク画像として使う
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キーを押して終了するのが良いでしょう．
実行結果
下図の様なウィンドウが表示されれば成功です．
ある特定の色だけが抜け落ちた画像が表示されます．
プログラム実行直後は，
- Rの範囲0〜128
- Gの範囲0〜128
- Bの範囲0〜128
この３つの条件に合致する画素値だけが取り出されています．
そのため(R,G,B)=(255,255,255)すなわち白色に近い方の色が抜け落ちています．
使い方
ウィンドウの下に追加されているトラックバー(スライダー)を操作して，色の範囲を変えてみましょう．
例えば，
- Rの範囲0〜255
- Gの範囲0〜255
- Bの範囲0〜255
として全部を指定すれば，元画像のままです．
それを，
- Rの範囲160〜210
- Gの範囲98〜152
- Bの範囲112〜164
に絞り込んでみると，この様にピンク色だけが取り出せました．
同様に，
- Rの範囲89〜109
- Gの範囲123〜191
- Bの範囲82〜117
黃緑色だけ取り出すこともできます．
この様に，自分が狙った色だけを抽出する際の最小値・最大値を決めるために，このプログラムを使います．
(3)TelloのHSV画像で二値化するサンプルプログラム
この章ではHSV空間で二値化をしてみます．
HSVへの変換や，cv2.inRangeによる範囲指定は既にやりましたから，少し変更するだけで対応が可能です．
main_hsv.py
プログラムはmain.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関数で範囲指定２値化 -> マスク画像として使う
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
の全ての範囲で下図の様に見える画像も，
- Hueの範囲161〜179
- Saturationの範囲81〜130
- Valueの範囲151〜193
と絞り込んでやると，この様になります．
やっている事は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を書き加える形で作成しました．
書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください．
#!/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関数で範囲指定２値化
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関数で範囲指定２値化
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関数は，黒で塗りつぶされた背景色全体も１つのラベルとして認識することです．
一般的なラベリングでは，値が0の画素は無視します．しかし，OpenCVのこの関数では１個目かつ最大面積のラベルは「背景全体」になるのです．(小さな親切・大きなお世話な機能)
この「背景全体」の情報は不要です．したがって，検出したラベル数を１つ減らし，stats,centerなどのリストから1番目の要素を削除する処理が絶対必要です．
参考：NumPyで任意の行・列を削除するdeleteの使い方
statsとcenterの配列の中身は，以下のようになっています．
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)
と書いて削除した結果を自分自身に代入し直しています．
この削除作業を忘れると，「一番大きな(=最大面積を持つ)ラベルを探す」という処理をすると，絶対に背景ラベルが１番になるという罠に陥ります(^^;;
次は，人間が見るためのイメージ(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に見せた際の処理結果は，こうなります．
検出したオブジェクト(ラベル)の重心位置と面積が表示されています．
また，カラーコーンのエッジ部分に現れた細かいゴミ(面積が1〜5程度)も検出してしまっています．本来このようなゴミは，膨張・収縮・孤立点除去などの前処理で落とすべきですね．
(5)物体を検出してTelloが追いかけるプログラム
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関数で範囲指定２値化
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には，以下の様にデータが入っています．
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の制御を行うプログラムです．
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はキーボード入力1と2で変更できるようになっています．
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(無効)になります．
フィードバック制御がONになると，Telloは左右旋回だけを行います．
操作手順
-
tキーで離陸させる． - 上下前後左右の移動キーで，ラベルが連続して認識できる位置(安全な位置)までTelloを手動操作する．
-
1キーを押してフィードバック制御を開始させる． - カラーコーンを左右に動かして，Telloが追従してくる事を確認する．
- もしTelloが意図しない方向へ流れ始め(暴走し)たら，
2キーを押して制御を終了させ，移動キーでTelloを止める． -
lキーで着陸させる．
Tello SDKのrcコマンドを使って操作しているので，機体が流れ始めた際に止めるのは手動操作だけです．（移動コマンドは応答が遅いので使っていません）
実行結果
以下の様に最大面積を持つラベルだけが表示されます．
離陸して，追跡させる物体を検出していることを確認したら，1キーを押して自動制御させます．
うまくいけば，冒頭でも紹介したこの動画の様にTelloが向きを変えます．
オレンジ色を２値化・ラベリング・重心計算してトラッキング。#tello pic.twitter.com/lx6aNiUEVg
— hsgucci404 (@hsgucci404) October 16, 2019
おわりに
今回は，OpenCVを使って二値化・ラベリング・面積/重心計算という，スタンダードな画像処理を試しました．
色の範囲を指定して二値化する手法は，周囲の明るさが大きく影響します．これを「環境条件に依存」「照明条件に依存」と呼びます．部屋の明るさが変わったり，屋外だと雲で日光が遮られたりすると，途端に検出できなくなることも多いです．
「環境変化にロバスト(頑健な)画像処理」というのは，ロボットのプログラマー皆の目標です．