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

ARマーカー認識プログラム

はじめに

このページは,

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

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

概要

今回は,TelloでARマーカーを検出し,マーカーの番号に応じて離着陸や移動などの動作をさせます.

OpenCVにはARマーカーのライブラリArUcoが標準で入っているので,比較的簡単に実装が可能です.

前提条件

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

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

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

ARマーカーって何?

ARマーカーとは,バーコードやQRコードの様な,データを持ったパターン画像の一種です.要は2次元バーコードです.

  ARマーカーの使いどころ バーコード・QRコード・ArUco・カメレオンコード

しかしARマーカーは,AR(Augmented Reality,拡張現実感)をデザインするために使うという目的があるので,「AR」の名を冠しています.

ARは,現実を撮影した画像の中に,さながら実物がそこにいるかの様にCGを組み合わせる技術です.スマホゲームでは,ポケモン(ポケモンGO)や恐竜(ジュラシックワールド)が,現実世界にいるかの様に見せる技術ですね.

PCではARToolKit,スマホではARKit,AR Coreといったライブラリを使って作ることが可能です

  例:ARToolKitを使った拡張現実感プログラミング

今回は,OpenCVのライブラリ ArUcoを使ってプログラミングをします.
参考にしたのはこのページです.

  PythonでARマーカー認識

contribモジュールのインストール

arucoモジュールはライセンスの都合でcontribとして分離されているので,「Tello-Video」のlinux_install.shでの記述$ sudo pip install opencv-python==3.4.5.20ではインストールされません.

  参考:OpenCV3.3(core+contrib)をPythonにインストール

以下のコマンドで,既存のopencv-python(3.4.5.20)をアンインストールし,

opencv-pythonのアンインストール
$ sudo pip uninstall opencv-python

contribも含んだopencvをインストールし直してください.

contribを含んだOpenCVのインストール
$ sudo pip install opencv-contrib-python==3.4.5.20

これで拡張モジュールを含んだOpenCVの全機能が使えるようになります.

TelloでARマーカーを認識するプログラム

ディレクトリの作成

まずは,Tello-Pythonディレクトリの下に,新しいディレクトリTello-CV-arを作ります.

Tello-CV-arディレクトリを作成
$ cd ~/Tello-Python/
$ mkdir Tello-CV-ar
$ cd Tello-CV-ar

ARマーカーの準備

まずは,ARマーカーを作り,印刷しなければなりません.

マーカーファイル出力プログラム

次のプログラムをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]などの機能で保存してください.

MakeMarker_0to9.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import cv2

# ArUcoのライブラリを導入
aruco = cv2.aruco

# 4x4のマーカー,ID番号は50までの辞書を使う
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

def main():
    # 10枚のマーカーを作るために10回繰り返す
    for i in range(10):

        ar_image = aruco.drawMarker(dictionary, i, 150)     # ID番号は i ,150x150ピクセルでマーカー画像を作る.

        fileName = "ar" + str(i).zfill(2) + ".png"  # ファイル名を "ar0x.png" の様に作る

        cv2.imwrite(fileName, ar_image)     # マーカー画像を保存する


# "python MakeMarker_0to9.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":
    main()  # メイン関数を実行

このプログラムを実行し,ARマーカーの画像ファイルを作ります.

ARマーカーの生成
$ python MakeMarker_0to9.py
$ ls
MakeMarker_0to9.py  ar01.png  ar03.png  ar05.png  ar07.png  ar09.png           main.py
ar00.png            ar02.png  ar04.png  ar06.png  ar08.png  libh264decoder.so  tello.py

プログラムを実行しても,特にメッセージ出力はされません.ですが,lsで見てみると,ar00.png〜ar09.pngまでの画像ファイルができていることがわかります.

一応,ここにも画像を貼っておきます.
ID=0        ID=1        ID=2        ID=3
ar00.png ar01.png ar02.png ar03.png

ID=4        ID=5        ID=6        ID=7
ar04.png ar05.png ar06.png ar07.png

ID=8        ID=9
ar08.png ar09.png

トレカに貼ると格好良い?

マーカー画像のファイルができたら,印刷しましょう.
印刷オプションで[自動的に拡大縮小]する機能は使わないでください.
滅茶苦茶大きいマーカーがプリントアウトされてきますので...(経験者は語る

マーカーは1枚ずつTelloに見せて使うので,カード状になっていると便利です.
筆者は,不要なトレーディングカードを貰ってきて,そこに貼り付けました.
ar_card_marker.png
ID番号順に並んでいない理由は,裏返すとこうなっているからです.
ar_card_func.png
IDと機能との対応は,

  • ID=0 : 離陸
  • ID=1 : 着陸
  • ID=2 : 上昇
  • ID=3 : 下降
  • ID=4 : 左旋回
  • ID=5 : 右旋回
  • ID=6 : 前進
  • ID=7 : 後進
  • ID=8 : 左移動
  • ID=9 : 右移動

となります.

ついでなので,トレカに貼り付けた裏側の画像も貼っておきます.
パワポで描いて印刷し,ハサミで切ってノリで貼っただけです(^_^
card.png
用意した機能は,キーボードでコントロールした時と同様ですね.
key_control.png

ファイルをコピー

tello.pyとlibh264decoder.soを,スケルトンのTello-CV-coreからコピーしてきましょう.

重要なファイルをコピー
$ cp ../Tello-CV-core/tello.py ./
$ cp ../Tello-CV-core/libh264decoder.so ./

main.pyもTello-CV-coreからコピーしておけば,書き加えるだけなので作業が楽ですね.

main.pyはスケルトンをコピー
$ cp ../Tello-CV-core/main.py ./

main.py

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

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

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

# メイン関数
def main():
    # OpenCVが持つARマーカーライブラリ「aruco」を使う
    aruco = cv2.aruco
    dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)   # ARマーカーは「4x4ドット,ID番号50まで」の辞書を使う

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

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

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

    pre_idno = None     # 前回のID番号を記憶する変数
    count = 0           # 同じID番号が見えた回数を記憶する変数

    #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用のカラー並びに変換する
            small_image = cv2.resize(image, dsize=(480,360) )   # 画像サイズを半分に変更

            # ARマーカーの検出と,枠線の描画
            corners, ids, rejectedImgPoints = aruco.detectMarkers(small_image, dictionary) #マーカを検出
            aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0)) #検出したマーカ情報を元に,原画像に描画する

            # 50回同じマーカーが見えたらコマンド送信する処理
            try:
                if ids != None: # idsが空(マーカーが1枚も認識されなかった)場合は何もしない
                    idno = ids[0,0] # idsには複数のマーカーが入っているので,0番目のマーカーを取り出す

                    if idno == pre_idno:    # 今回認識したidnoが前回のpre_idnoと同じ時には処理
                        count+=1            # 同じマーカーが見えてる限りはカウンタを増やす

                        if count > 50:      # 50回同じマーカーが続いたら,コマンドを確定する
                            print("ID=%d"%(idno))

                            if idno == 0:
                                drone.takeoff()             # 離陸
                            elif idno == 1:
                                drone.land()                # 着陸
                                time.sleep(3)
                            elif idno == 2:
                                drone.move_up(0.3)          # 上昇
                            elif idno == 3:
                                drone.move_down(0.3)        # 下降
                            elif idno == 4:
                                drone.rotate_ccw(20)        # 左旋回
                            elif idno == 5:
                                drone.rotate_cw(20)         # 右旋回
                            elif idno == 6:
                                drone.move_forward(0.3)     # 前進
                            elif idno == 7:
                                drone.move_backward(0.3)    # 後進
                            elif idno == 8:
                                drone.move_left(0.3)        # 左移動
                            elif idno == 9:
                                drone.move_right(0.3)       # 右移動

                            count = 0   # コマンド送信したらカウント値をリセット
                    else:
                        count = 0

                    pre_idno = idno # 前回のpre_idnoを更新する
                else:
                    count = 0   # 何も見えなくなったらカウント値をリセット

            except ValueError, e:   # if ids != None の処理で時々エラーが出るので,try exceptで捕まえて無視させる
                print("ValueError")


            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', small_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()    # メイン関数を実行

プログラムの実行

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

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

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

実行結果

TelloにARマーカーを1つずつ見せてください.複数同時には対応できていませんので.
また,Telloが動き出したら,すぐにカードを引っ込めてください.「見せっぱなし」にすると思いもよらない動きをします

また,imshowのOpenCVウィンドウがアクティブならば,スケルトンと同様にキー入力による操縦が可能です.

問題なく動作すれば,以下の様になるはずです.

実行結果
$ python main.py 
sent: command
sent: streamon
[h264 @ 0x1ed7b00] non-existing PPS 0 referenced
[h264 @ 0x1ed7b00] non-existing PPS 0 referenced
[h264 @ 0x1ed7b00] decode_slice_header error
[h264 @ 0x1ed7b00] no frame!
()
ID=0                        <- ID=0のカードを認識したのでtakeoffコマンドを送信
>> send cmd: takeoff
ID=0
>> send cmd: takeoff
>> send cmd: command        <- 5秒ごとのcommand
ID=2
>> send cmd: up 30
>> send cmd: command
ID=2
>> send cmd: up 30
>> send cmd: command
ID=2
>> send cmd: up 30
ID=5
>> send cmd: cw 20
>> send cmd: command
ID=5
>> send cmd: cw 20
>> send cmd: command

カードを認識した際に,コンソールにID番号が表示されます.

実際に動かしている様子の動画は,以下の様になります.

main.pyの解説

メイン関数以外は,前回のスケルトンプログラムと同じなので,説明は割愛します.

import部分

インポート部分は特に変わりはありません.

インポート
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため

ArUcoのライブラリはOpenCVが持っているので,cv2のインポートで済んでしまいます.

メイン関数

メイン関数の中身は大きく分けて3つの部分に分かれています.
「初期化」「ループ」「終了処理」です.

メイン関数
# メイン関数本体
def main():
    初期化部

    ループ部

    終了処理部

それぞれ解説していきます.

初期化部

初期化処理部
    # OpenCVが持つARマーカーライブラリ「aruco」を使う
    aruco = cv2.aruco
    dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)   # ARマーカーは「4x4ドット,ID番号50まで」の辞書を使う

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

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

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

    pre_idno = None     # 前回のID番号を記憶する変数
    count = 0           # 同じID番号が見えた回数を記憶する変数

Telloクラス生成や,5秒おきにcommandを送信する機能はいつも通りです.

今回はArUcoの機能を使うので,cv2.arucoでクラスを作っています.
また「4x4ドット,ID=50まで」の辞書を呼び出しています.これは,ARマーカーを作った時と同じ辞書を使わなければ認識できないので注意してください.画像生成側と画像読込側で同じ辞書を使う必要があります

また,pre_idno(前回のIDナンバー)とcount(回数を数えるカウンタ)という変数を定義しています.
これは,ARマーカーが一瞬見えただけではTelloを動かさないようにするためです.たった1フレームマーカーが見えただけで移動コマンドを送信していたら,見えている間連続してコマンドが送信され先行入力が溜まって暴走する危険性があります.

今回は50回同じマーカーが見え続けたらコマンド送信をするようにしました.そのための変数がpre_idnocountです.

ループ部

while Trueで永久ループを作り,ctrl+cを検知をtry exceptでやるのは今までと同様です.

永久ループ部
    #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用のカラー並びに変換する
            small_image = cv2.resize(image, dsize=(480,360) )   # 画像サイズを半分に変更
            # ARマーカーの検出と,枠線の描画
            corners, ids, rejectedImgPoints = aruco.detectMarkers(small_image, dictionary) #マーカを検出
            aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0)) #検出したマーカ情報を元に,原画像に描画する
            # 50回同じマーカーが見えたらコマンド送信する処理
            try:
                if ids != None: # idsが空(マーカーが1枚も認識されなかった)場合は何もしない
                    idno = ids[0,0] # idsには複数のマーカーが入っているので,0番目のマーカーを取り出す
                    if idno == pre_idno:    # 今回認識したidnoが前回のpre_idnoと同じ時には処理
                        count+=1            # 同じマーカーが見えてる限りはカウンタを増やす
                        if count > 50:      # 50回同じマーカーが続いたら,コマンドを確定する
                            print("ID=%d"%(idno))

                            if idno == 0:
                                drone.takeoff()             # 離陸
                            elif idno == 1:
                                drone.land()                # 着陸
                                time.sleep(3)
                            ()
                            count = 0   # コマンド送信したらカウント値をリセット
                    else:
                        count = 0
                    pre_idno = idno # 前回のpre_idnoを更新する
                else:
                    count = 0   # 何も見えなくなったらカウント値をリセット
            except ValueError, e:   # if ids != None の処理で時々エラーが出るので,try exceptで捕まえて無視させる
                print("ValueError")

            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', small_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()                # 着陸
            ()

            # (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-CV-coreのmain.pyと比べてもらうと分り易いですが,(B)のブロックに色々と書き加えられているだけです.(A),(X),(Y),(Z)はそのままです.

次は(B)ブロックだけ見ていきます.

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

            # ARマーカーの検出と,枠線の描画
            corners, ids, rejectedImgPoints = aruco.detectMarkers(small_image, dictionary) #マーカを検出
            aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0)) #検出したマーカ情報を元に,原画像に描画する

            # 50回同じマーカーが見えたらコマンド送信する処理
            try:
                if ids != None: # idsが空(マーカーが1枚も認識されなかった)場合は何もしない
                    idno = ids[0,0] # idsには複数のマーカーが入っているので,0番目のマーカーを取り出す

                    if idno == pre_idno:    # 今回認識したidnoが前回のpre_idnoと同じ時には処理
                        count+=1            # 同じマーカーが見えてる限りはカウンタを増やす

                        if count > 50:      # 50回同じマーカーが続いたら,コマンドを確定する
                            print("ID=%d"%(idno))

                            if idno == 0:
                                drone.takeoff()             # 離陸
                            elif idno == 1:
                                drone.land()                # 着陸
                                time.sleep(3)
                            elif idno == 2:
                                drone.move_up(0.3)          # 上昇
                            elif idno == 3:
                                drone.move_down(0.3)        # 下降
                            elif idno == 4:
                                drone.rotate_ccw(20)        # 左旋回
                            elif idno == 5:
                                drone.rotate_cw(20)         # 右旋回
                            elif idno == 6:
                                drone.move_forward(0.3)     # 前進
                            elif idno == 7:
                                drone.move_backward(0.3)    # 後進
                            elif idno == 8:
                                drone.move_left(0.3)        # 左移動
                            elif idno == 9:
                                drone.move_right(0.3)       # 右移動

                            count = 0   # コマンド送信したらカウント値をリセット
                    else:
                        count = 0

                    pre_idno = idno # 前回のpre_idnoを更新する
                else:
                    count = 0   # 何も見えなくなったらカウント値をリセット

            except ValueError, e:   # if ids != None の処理で時々エラーが出るので,try exceptで捕まえて無視させる
                print("ValueError")

画像をBGR並びにして縮小し,small_imageという画像を作るまでは,Tello-CV-coreと同じです.

ARマーカーの認識をaruco.detectMarkers(small_image, dictionary)で行っています.small_image画像を,初期化部で用意したAR辞書dictionaryで処理しています.

detectMarkersの処理結果を使って,aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0))で画面上に枠の描画を行っています.今回は(B,G,R)=(0,255,0)の色です.
OpenCVを使って画像処理をする最低動作プログラム「スケルトンプログラム」を作成しまし
検出できた全てのマーカーのIDがidsに入っているので,1番先頭に入っているマーカーを使うことにしました.
このプログラムをもっと拡張するのであれば,複数枚見えた時は「一番大きく見えているマーカーを使う」,という処理にすれば,より汎用性が向上すると思います.その場合,cornersの中身から一番大きな枠を作るインデックスを探す処理を追加する必要があります.

1フレーム前に検出したID番号と,今回検出したID番号と番号を比較して,同じ時だけ作業を行います.
カードが代わったらリセットです.

同じカードを50回検出することができたら,Telloへのコマンド送信を行っています.
フローで書くと,こんな感じです.
ar_flow.png

この50回という回数を変化させることで,反応を早くしたり遅くしたりできます.
「何回連続して検出したかをカウントする」というのは,よくあるプログラミングのテクニックです.「キーボードやジョイパッドボタンの長押し」の検出でもよく使われます.もちろん,検出処理をもっとスマートに書くこともできますが,今回は入門なので「シンプル・読み易く」を追求しました.

新しいコマンドを増やしたい場合,ここのelif文でID番号を追加します.「(前後左右への)宙返り」のコマンドを追加できそうですね.

おわりに

今回はArUcoライブラリを使ってARマーカーを認識し,Telloに見せるとTelloが動く,というプログラムを作りました.
これ,子供にやらせると結構喜んでくれますよ(^^

今回はARマーカー本来の使い方「映像にCGを重ねる」とは違いますが,ARマーカー認識の基本となります.

カメラキャリブレーションや,マーカーの角度検出などへと発展すれば,部屋や障害物にマーカーを貼ることでロボットの自己位置を予想する事も可能になります.
参考ページを貼っておきます.

 ・pythonでカメラ行列と歪みパラメータの取得
 ・pythonでARマーカーの姿勢推定

「ID番号xxのマーカーが角度r,p,yで見えているという事は,自分は今x,yの位置にいることになる.」
「ID番号xxのマーカーは荷物だ.これを持ち上げる必要がある.」
なんて使い方ができるわけです.

 

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