色検出プログラム

#はじめに
このページは，

[公式SDK「Tello-Python」を試そう](https://qiita.com/hsgucci/items/3327cc29ddf10a321f3c)

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

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

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

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

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

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

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

今回は<font color="blue">**OpenCVのラベリング関数を使って，色を基準として物体を識別しTelloで追跡**</font>させてみます．

具体的には，この動画の様に，オレンジ色のカラーコーンを追いかけさせます．
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">オレンジ色を２値化・ラベリング・重心計算してトラッキング。<a href="https://twitter.com/hashtag/tello?src=hash&amp;ref_src=twsrc%5Etfw">#tello</a> <a href="https://t.co/lx6aNiUEVg">pic.twitter.com/lx6aNiUEVg</a></p>&mdash; hsgucci404 (@hsgucci404) <a href="https://twitter.com/hsgucci404/status/1184409855643144192?ref_src=twsrc%5Etfw">October 16, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 
カラーコーンを追いかけて，左右旋回をさせるだけの簡単なプログラムですが，バックグラウンドには多くの要素技術があるので，記事が長めになっています．


#前提条件

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

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

詳しくは　[Tello-Pythonのダウンロード](https://qiita.com/hsgucci/items/3327cc29ddf10a321f3c#tello-python%E3%81%AE%E3%83%80%E3%82%A6%E3%83%B3%E3%83%AD%E3%83%BC%E3%83%89)　を御覧ください．

#今回の作業内容

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

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

#作業ディレクトリの作成

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

```python: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の画像は**<font color="red">赤</font><font color="green">緑</font><font color="blue">青</font>**のRGB３色ですが，色をキーにして物体検出をさせる時は，**色相(Hue)・彩度(Saturation)・明度(Value)**のHSV色空間を使ったほうが検知し易いです．

　　参考：[【python/OpenCV】画像の特定の色を抽出する方法](http://rikoubou.hatenablog.com/entry/2019/02/21/190310)
　　　　　[色空間の変換](http://labs.eecs.tottori-u.ac.jp/sd/Member/oyamada/OpenCV/html/py_tutorials/py_imgproc/py_colorspaces/py_colorspaces.html)

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


##main_color.py

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

書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_CV_color/main_colors.py)　を右クリックして[名前を付けて保存]機能でファイル保存してください．

```python: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)ブロックのみです．

```python:スケルトンからの変更箇所
			# (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:プログラムの実行
$ python main_color.py
```
今までと同様に`ctrl+c`を押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウで`ESC`キーを押して終了するのが良いでしょう．

##実行結果

下図の様に，２枚の画像が表示されれば成功です．
![bgr_hsv.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/d1694b80-7dde-3184-a786-77decd4bb09b.png)
元画像であるBGRのウィンドウと，HSVを無理やりBGR化して表示したウィンドウが表示されています．

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

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

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

本来，二値化というのは，ある**しきい値(閾値)**を境目にして，それ未満の画素値だったらゼロ・それ以上の画素値だったら255，の２つの値にする処理のことです．`cv2.threshold`関数を使うことで実現できます．
　　参考：[OpenCV 画像の二値化](https://qiita.com/tokkuri/items/ad5e858cbff8159829e9)

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

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

　　参考[OpenCV - inRange による範囲指定で2値化する方法について](https://www.pynote.info/entry/opencv-inrange)

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

##main_bgr.py

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

書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_CV_color/main_bgr.py)　を右クリックして[名前を付けて保存]機能でファイル保存してください．

```python: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関数で範囲指定２値化 -> マスク画像として使う
			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:プログラムの実行
$ python main_bgr.py
```
今までと同様に`ctrl+c`を押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウで`ESC`キーを押して終了するのが良いでしょう．

##実行結果
下図の様なウィンドウが表示されれば成功です．
![tello_color_bgr.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/dfe782c1-210f-4908-4951-c64a95fd8666.png)
ある特定の色だけが抜け落ちた画像が表示されます．

プログラム実行直後は，

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

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

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

例えば，

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

として全部を指定すれば，元画像のままです．
![tello_color_sample.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/805777aa-e207-2153-7bd3-e5279ec90055.png)

それを，

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

に絞り込んでみると，この様にピンク色だけが取り出せました．
![tello_color_sample_pink.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/83312bcb-225d-2d72-d934-8c7bcecfaa24.png)

同様に，

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

黃緑色だけ取り出すこともできます．
![tello_color_sample_green.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/bd92ea3b-3a01-6602-1291-3151bcabee11.png)

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


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

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

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

##main_hsv.py

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

書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_CV_color/main_hsv.py)　を右クリックして[名前を付けて保存]機能でファイル保存してください．

```python: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関数で範囲指定２値化 -> マスク画像として使う
			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:プログラムの実行
$ python main_hsv.py
```
今までと同様に`ctrl+c`を押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウで`ESC`キーを押して終了するのが良いでしょう．


##実行結果
例えば，

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

の全ての範囲で下図の様に見える画像も，
![tello_color_sample_hsv.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/fa3ae8aa-d16a-e274-6013-5d91a746a9d7.png)


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

と絞り込んでやると，この様になります．
![tello_color_sample_hsv_pink.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/ec431c29-c25d-83e0-fef9-37f866184e04.png)


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

というのは，Hueは色相ですから，色相環をぐるっと一周回れば全ての色が表現できるわけです．つまり，Hueだけで色がほぼ特定できることになります．
　　参考：[色相](https://ja.wikipedia.org/wiki/%E8%89%B2%E7%9B%B8)
余談ですが，本来の色相は0〜360で一周ですが，OpenCVでは半分にして表現しています．255以上の数値は8ビットでは足りなくなってしまうので．

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

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

参考
　　[Python+OpenCVでラベリング](http://okkah.hateblo.jp/entry/2018/08/02/163045)
　　[OpenCV - connectedComponents() で連結成分のラベリング](http://pynote.hatenablog.com/entry/opencv-connected-components-labeling)
　　[Python+OpenCVを利用したラベリング処理](https://dronebiz.net/tech/opencv/labeling)
　　[[OpenCV Python]OpenCVを使ったラベリング](https://github.com/atinfinity/lab/wiki/%5BOpenCV-Python%5DOpenCV%E3%82%92%E4%BD%BF%E3%81%A3%E3%81%9F%E3%83%A9%E3%83%99%E3%83%AA%E3%83%B3%E3%82%B0)
　　[【Python/OpenCV】最大面積のブロブ解析（座標・大きさなど）](https://algorithm.joho.info/programming/python/blob-max-moment/)

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



##main_label.py

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

書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_CV_color/main_label.py)　を右クリックして[名前を付けて保存]機能でファイル保存してください．

```python: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関数で範囲指定２値化
			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二値化とほぼ同じですが，ラベリング処理と処理結果表示を追加しました．

```python:ラベリング処理
			# 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の方です．

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

この「背景全体」の情報は不要です．したがって，検出したラベル数を１つ減らし，`stats`,`center`などのリストから1番目の要素を削除する処理が絶対必要です．
　　参考：[NumPyで任意の行・列を削除するdeleteの使い方](https://note.nkmk.me/python-numpy-delete/)

`stats`と`center`の配列の中身は，以下のようになっています．

```python:ラベリング結果の配列の中身
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ループを回しています．

```python:ラベリング処理の結果を元に，枠線を描いたり，テキストを描いたりする部分
			# 検出したラベルの数だけ繰り返す
			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:プログラムの実行
$ python main_label.py
```
今までと同様に`ctrl+c`を押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウで`ESC`キーを押して終了するのが良いでしょう．

##実行結果
![tello_label_sample.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/52be303c-63d6-6ed9-7b31-dbded87a3e4c.png)
上図の様に，複数のカラーコーンをTelloに見せた際の処理結果は，こうなります．
![tello_label_result.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/2788ba2c-019c-d41e-9128-2105369c8850.png)

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

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

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

##main_control.py

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

書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_CV_color/main_control.py)　を右クリックして[名前を付けて保存]機能でファイル保存してください．

```python: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`には，以下の様にデータが入っています．

```python: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の参照、抽出、結合/列を抽出する](https://qiita.com/supersaiakujin/items/d63c73bb7b5aac43898a#%E5%88%97%E3%82%92%E6%8A%BD%E5%87%BA%E3%81%99%E3%82%8B)

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

```python:最大面積を持つラベルを抽出する
			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の制御を行うプログラムです．

```python:自動制御フラグが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はキーボード入力`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に，不感帯とソフトウェアリミッタの処理をかけています．
三項演算子を使った不感帯・ソフトリミッタ処理についての詳しくは， [こちら](https://qiita.com/hsgucci/items/5d4e4de933a14820795b#b%E7%94%BB%E5%83%8F%E5%87%A6%E7%90%86%E3%83%96%E3%83%AD%E3%83%83%E3%82%AF) で説明しています．

##プログラムの実行

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

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

###操作系

操作系は以下の様になっています．
`1`キーで色追従のフィードバック制御がON(有効)になり，
`2`キーでOFF(無効)になります．
![cv_face_key.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/0e6eccf0-ee26-49ae-7413-10889ef34d7b.png)

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

###操作手順

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

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


##実行結果

以下の様に最大面積を持つラベルだけが表示されます．
![tello_label_control.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/fbe2b2ac-44ba-9195-acd9-b36f7eed6d90.png)

離陸して，追跡させる物体を検出していることを確認したら，`1`キーを押して自動制御させます．
うまくいけば，冒頭でも紹介したこの動画の様にTelloが向きを変えます．
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">オレンジ色を２値化・ラベリング・重心計算してトラッキング。<a href="https://twitter.com/hashtag/tello?src=hash&amp;ref_src=twsrc%5Etfw">#tello</a> <a href="https://t.co/lx6aNiUEVg">pic.twitter.com/lx6aNiUEVg</a></p>&mdash; hsgucci404 (@hsgucci404) <a href="https://twitter.com/hsgucci404/status/1184409855643144192?ref_src=twsrc%5Etfw">October 16, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 



#おわりに

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

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

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