はじめに

Qiita初投稿です．
なんちゃって情報工学科生が，PythonとOpenCVとドローンを使ってARマーカを生成・検出し，姿勢推定をする方法について，超ざっくり書こうと思います．

こちらのブログを参考にさせていただきました．
https://qiita.com/ReoNagai/items/a8fdee89b1686ec31d10

実装

ArUcoマーカの生成

今回は，OpenCVのモジュールの一つであるAR用ライブラリArUcoを使用します．
ArUcoマーカは以下のコードで生成できます．

generate.py

#!/usr/bin/env python
# -*- coding: utf-8 -*
import cv2
aruco = cv2.aruco
dir(aruco)

dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

marker = aruco.drawMarker(dictionary, 1, 300)
cv2.imwrite('id-001.png', marker)

aruco.DICT_4X4_50 で50種類のマーカが生成可能です．
とりあえず1個生成してみるとこんな感じです．

印刷して壁に貼るなり何なりします．
私はドローンの追尾に使っていたので，このようにドローンの後部に貼り付けました．

ArUcoマーカの検出

ドローンのカメラから検出する場合

parrot社のAR.Drone2.0を使用します．

Parrot AR. Drone2.0，
https://www.parrot.com/jp/doron/parrot-ardrone-20-power-edition (2020.4.22 access)

detect_drone.py

# -*- coding: utf-8 -*-
import numpy as np
import cv2 
import sys
from pyardrone import ARDrone
import logging

logging.basicConfig(level=logging.DEBUG)

client = ARDrone()
client.video_ready.wait()
cnt=0

aruco = cv2.aruco #arucoライブラリ
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

try:
    while True:
        cv2.imwrite('drone'+str(cnt)+'.png',client.frame)
        cnt+=1
        corners, ids, rejectedImgPoints = aruco.detectMarkers(client.frame, dictionary)    #マーカを検出
        aruco.drawDetectedMarkers(client.frame, corners, ids, (0,255,0))   #検出したマーカに描画
        if cv2.waitKey(10) == ord(' '):
            break
finally:
    client.close()

一般的なwebカメラなどでやる場合

detect_camera.py

# -*- coding: utf-8 -*-
import numpy as np
import cv2 
import sys
 
aruco = cv2.aruco #arucoライブラリ
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

def arReader(): 
    print(cv2.getBuildInformation())
    cap = cv2.VideoCapture(0)
    cnt=0

    while True:
 
        ret, frame = cap.read()
        if frame is None: break
 
        corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary) #マーカを検出
        print(corners,ids)
        aruco.drawDetectedMarkers(frame, corners, ids, (0,255,0)) #検出したマーカに描画

        cv2.imwrite('result'+str(cnt)+'.png',frame)
        cnt+=1

    cap.release()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    arReader()

実行結果

このように，マーカの枠線・ID・コーナー・XYZ軸が描画され，検出ができました．

マーカ-カメラ間の距離や傾きを測定

マーカ-カメラ間の距離や傾きを求めたい時は，一般的に赤外線センサなどを用いることが多いと思いますが，今回はカメラキャリブレーションによって求めたカメラの内部パラメータや歪み係数の他に，回転行列や回転ベクトル，並進ベクトルを使って求めていきます．

カメラキャリブレーション

まずカメラキャリブレーションを行い，カメラの内部パラメータと歪み係数を求めます．

calib.py

# -*- coding: utf-8 -*
import numpy as np
import cv2
import glob

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

objp = np.zeros((7*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:7].T.reshape(-1,2)

objpoints = []
imgpoints = []

gray_images=glob.glob('chess*.png')
cnt=0

for fname in gray_images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    ret, corners = cv2.findChessboardCorners(gray, (7,7),None)

    if ret == True:
        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        img = cv2.drawChessboardCorners(img, (7,7), corners2,ret)
        cv2.imwrite('calib'+str(cnt)+'.png',img)
        cnt+=1

    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

print(mtx, dist)

cv2.destroyAllWindows()

実行結果

カメラの内部パラメータ
[[9.31357583e+03 0.00000000e+00 1.61931898e+03]
[0.00000000e+00 9.64867367e+03 1.92100899e+03]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
歪み係数
[[ 0.22229833 -6.34741982  0.01145082  0.01934784 -8.43093571]]

ロドリゲスで計算

回転行列や回転ベクトル，並進ベクトルを使って計算していきます．
camera_matrix は上の方法で求めたパラメータに書き換えてください．

distance.py

#!/usr/bin/env python
# -*- coding: utf-8 -*
import cv2
import numpy as np
import sys
from pyardrone import ARDrone
import logging

logging.basicConfig(level=logging.DEBUG)

aruco = cv2.aruco #arucoライブラリ
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

client = ARDrone()
client.video_ready.wait()

parameters =  aruco.DetectorParameters_create()

parameters.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR


def main():
    cnt=0
    marker_length = 0.1 # [m]

    camera_matrix = np.array( [[ , , ],
                               [ , , ],
                               [ , , ]] )
    #カメラキャリブレーションでパラメータを求める↑

    distortion_coeff = np.array( [[ 0.22229833, -6.34741982,  0.01145082,  0.01934784, -8.43093571]] )

    try:
        while True:
            corners, ids, rejectedImgPoints = aruco.detectMarkers(client.frame, dictionary, parameters=parameters)

            aruco.drawDetectedMarkers(client.frame, corners, ids, (0,255,255))

            if len(corners) > 0:
                for i, corner in enumerate(corners):
                    # rvec -> rotation vector, tvec -> translation vector
                    rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)

                    tvec = np.squeeze(tvec)
                    rvec = np.squeeze(rvec)
                    rvec_matrix = cv2.Rodrigues(rvec)
                    rvec_matrix = rvec_matrix[0]
                    transpose_tvec = tvec[np.newaxis, :].T
                    proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
                    euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6] # [deg]
                    print("x : " + str(tvec[0]))
                    print("y : " + str(tvec[1]))
                    print("z : " + str(tvec[2]))
                    print("roll : " + str(euler_angle[0]))
                    print("pitch: " + str(euler_angle[1]))
                    print("yaw  : " + str(euler_angle[2]))

                    draw_pole_length = marker_length/2 # 現実での長さ[m]
                    aruco.drawAxis(client.frame, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)

            cv2.imwrite('distance'+str(cnt)+'.png',client.frame)
            cnt+=1

            key = cv2.waitKey(50)
            if key == 27: # ESC
                break
    finally:
        client.close()

if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        pass

実行結果

正しく距離を測定できているのかを確かめるために，マーカ-カメラ(ドローン)間を25cmと50cmにして，プログラムを実行してみます．

実行結果(25cm)

x : -0.5410338091817037 
y : -0.713790809892646
z : 3.828983632577233
roll : [173.43049198]
pitch: [-20.42010647]
yaw  : [1.58947772]

実行結果(50cm)

x : -1.0633298519994792 
y : -1.3476793013004273
z : 7.311742619516305
roll : [-150.91884043]
pitch: [2.45488175]
yaw  : [3.61554034]

良い感じに実行結果の値が倍になっています．
このようにして，XYZ軸それぞれに動いた大きさとロールピッチヨーの値が求まります．

おまけ

求めた値から，以下の表のように条件分岐してドローンを追尾させました．

マーカ-カメラ間の距離	ドローンの動作
~0.5m	ホバリング
0.5m ~ 2.0m	前進
2.0m ~	着陸
それ以外	ホバリング

構成図はこんな感じです．

マルチホップ通信(化石)をさせるので，RasPiさえドローンに載せることができれば人が立ち入れない場所の調査端末とかになるんじゃねということをやっていました．
そのために大量のRasPiの無線AP化だったりdnsmasqやhostapdの設定をするんですが，これまた面倒すぎるのでまたの機会に書こうと思います．

おわりに

今回は，PythonとOpenCVを使ってARマーカを生成・検出し、距離と傾きを求める方法についてやりました．
このArUcoマーカ，手軽で正確な認識が可能で，マーカ-カメラ間の距離やマーカの向き，それぞれに割り振られた番号(ID)が分かる優れものだと思うんですが，あまり有名じゃない…のか…？
いつかドローン編も書きます．