OpenCVは画像処理に触れたことのある人ならば知らない人はいないと言われるほど有名なライブラリですが、本日はそのモジュールの一つである、AR用ライブラリArUcoを用いて、物体の位置計測をしてみます。
ネットにはC++の記事がたくさん転がっているのですが、Pythonを用いた記事が少ないように感じたため、備忘録としてブログに残そうと思った次第です。

やりたいこと

机の上に座標系を設定し（横の辺が$x$軸、縦の辺が$y$軸とか）、机の上を動き回る物体（小型ロボットとか）の位置座標を、机を撮影するカメラの映像から推定したいです。

準備

PCにPythonはインストールされているとし、OpenCVのインストールから行います。

pip install opencv-contrib-python

動作確認を行いましょう。

import cv2
aruco = cv2.aruco
help(aruco)

それっぽい文章がわちゃわちゃ出てきたら、インストール成功です。

マーカー生成

ArUcoでは、QRコードのような2次元マーカーを生成し、画像中から生成されたマーカーを認識することができます。画像の生成は以下のように行います。

dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
for i in range(5):
  marker = aruco.drawMarker(dictionary, i+1, 100)
  cv2.imwrite('ar_marker'+str(i+1)+'.png', marker)

ここで、getPredefinedDictionaryはマーカーが格納されている辞書を呼び出す関数であり、DICT_4X4_50は辞書の種類を表します。また、forループの中では、drawMarkerでidが$i+1$のマーカーを呼び出し、imwriteで呼び出したマーカーを保存しています。

生成された画像はつぎのようになります。

marker

今回マーカーは5つ呼び出しました。 1つは物体に設置し、認識するためのもので、残りの4つは座標変換のためのものです。

マーカー検出

画像中のマーカーを検出するためのコードが以下です。

img = cv2.imread('img.jpg')
corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary)
img_marked = aruco.drawDetectedMarkers(img, corners, ids)
cv2.imwrite('img_marked.png', img_marked)

detectMarkersで、画像からマーカーを認識します（たった一行！）。戻り値であるcornersとidsは、それぞれ検出されたマーカーの座標とidが格納されたリストです。

先ほど生成された画像をモニタに表示させ、手元のカメラで撮影したものを認識させてみました。結果がつぎのようになります。

recognition

座標変換

ArUcoでは、画像中のマーカーの位置を検出することができますが、その位置座標は机の上の座標と必ずしも一致しません。このため、机の上に座標変換用のマーカーを設置し、それらを用いて座標変換を行うことにします。この作業は、ArUcoではなく、OpenCVライブラリによって実現できます。

marker_coordinates = np.float32(moments)
true_coordinates = np.float32([[0.,0.],[0.,1000.],[1000.,0.],[1000.,1000.]])
trans_mat = cv2.getPerspectiveTransform(marker_coordinates,true_coordinates)
img_trans = cv2.warpPerspective(original_img,trans_mat,(width, height))
cv2.imwrite('img_trans.png', img_trans)

marker_coordinatesは、detectMarkersで検出された、座標変換用のマーカーの座標を表します。また、true_coordinatesは、それらの座標が、机の上のどの座標に対応するかを表します。これら2つの座標変換を行うためには、getPerspectiveTransformという関数を用います（これもたった一行！）。

先の画像を座標変換した結果がつぎです。（上下が逆になっていますが、）上から撮影したように画像が変換されていることがわかります。

transform

実践

さて、ここまで紹介した関数を用いて、あらかじめ記録しておいた動画を1フレームずつ読み込み、マーカーの認識と座標変換を行い、物体の位置計測をするコードを書いてみました。

#%%
import numpy as np
import cv2
aruco = cv2.aruco
import matplotlib.pyplot as plt

WINDOW_NAME = "window"
ORG_FILE_NAME = "IMG_0888.MOV"
NEW_FILE_NAME = "new.avi"


#%%
def calcMoments(corners,ids):
    moments = np.empty((len(corners),2))
    for i in range(len(corners)):
        index = int(ids[i])-1
        moments[index] = np.mean(corners[i][0],axis=0)
    return moments


def transPos(trans_mat,target_pos):
    target_pos = np.append(target_pos,1)
    target_pos_trans = trans_mat@target_pos
    target_pos_trans = target_pos_trans/target_pos_trans[2]
    return target_pos_trans[:2]


#%%
org = cv2.VideoCapture(ORG_FILE_NAME)
end_flag, original_img = org.read()
width = 1000
height = 1000
fourcc = cv2.VideoWriter_fourcc(*'XVID')
rec = cv2.VideoWriter(NEW_FILE_NAME,fourcc, 20.0, (width, height))
cv2.namedWindow(WINDOW_NAME)


#%%
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
corners, ids, rejectedImgPoints = aruco.detectMarkers(original_img, dictionary)
img_marked = aruco.drawDetectedMarkers(original_img, corners, ids)
cv2.imwrite('detect.png', img_marked)
print(all(ids>5))


#%%
while(1):
    corners, ids, rejectedImgPoints = aruco.detectMarkers(original_img, dictionary)
    if ids.all()!=None and ids.size>4:
        break
    end_flag, original_img = org.read()
moments = calcMoments(corners,ids)

marker_coordinates = np.float32(moments[:4])
true_coordinates = np.float32([[0.,0.],[width,0.],[0.,height],[width,height]])
trans_mat = cv2.getPerspectiveTransform(marker_coordinates,true_coordinates)
img_trans = cv2.warpPerspective(original_img,trans_mat,(width, height))
cv2.imwrite('trans.png', img_trans)


#%%
x_t = []
y_t = []
while end_flag == True:
    corners, ids, rejectedImgPoints = aruco.detectMarkers(original_img, dictionary)
    if ids.all()!=None and ids.size==5 and all(ids<=5):
        moments = calcMoments(corners,ids)
        marker_coordinates = np.float32(moments[:4])
        trans_mat = cv2.getPerspectiveTransform(marker_coordinates,true_coordinates)

        target_pos = moments[4]
        trans_pos = transPos(trans_mat,target_pos)

        x_t.append(trans_pos[0])
        y_t.append(trans_pos[1])

        img_marked = aruco.drawDetectedMarkers(original_img, corners, ids)
        img_trans = cv2.warpPerspective(img_marked,trans_mat,(width, height))
    else:
        x_t.append(None)
        y_t.append(None)
        img_trans = cv2.warpPerspective(original_img,trans_mat,(width, height))

    cv2.imshow(WINDOW_NAME, img_trans)
    rec.write(img_trans)

    end_flag, original_img = org.read()

cv2.destroyAllWindows()
org.release()
rec.release()


#%%
fig = plt.figure()
plt.scatter(x_t, y_t)
plt.xlabel('x')
plt.ylabel('y')
plt.savefig("tragectory.png", dpi=300)
plt.show()

上記のコードを、つぎの動画に適用した結果が、 original_movie

こちらになります。 new_img

先ほどと同じく上下が逆になっていますが、座標変換によって手ブレが抑えられ、さらに位置計測もできていることがわかります。

計測された位置の奇跡を表示してみましょう。 tragectory

しっかり位置が計測できていることがわかります。

やりたいこと#

準備#

マーカー生成#

マーカー検出#

座標変換#

実践#

やりたいこと

準備

マーカー生成

マーカー検出

座標変換

実践