Raspberry PiでArUcoを使ってみる

11193 ワード
AR ArUco RaspberryPi Python 画像認識 Python テキストリンク

Raspberry PiでArUcoを使ってみる

少し古い技術ですが、ロボット系の自己位置推定で見かけることがある
ARマーカーを試してみたくなり
入手したRaspberryPiでArUcoを認識させてみました。

やったこと

実行結果の見ためはこんな感じ。(https://www.youtube.com/watch?v=aepFM_JsxbU)
マーカーの輪郭とID,xyz軸を表示しています。　ピッチロールヨーも取得できるので
様々な用途に使えそう。

環境
・RasPi4　(RasPi3でも動くはずです)
・USBカメラ(Logicool)　→　Raspi camでも可能。

RasPiのセットアップ

まずは”からあげ”さんの下記の記事通りセットアップ。
認識にはOpenCVが必要です。
非常にスムーズにできました。いつも感謝です。
・Raspberry Pi 4のディープラーニングで画像認識する環境をゼロから1時間で構築する方法

追加でパッケージをインストール
記事通り、いったん全部入れました。
・Raspberry Pi 4の強化学習用のセットアップ

これで準備完了。

ARマーカーを認識する手順

マーカーの生成　→生成は下記リンク参照.
・(カメラキャリブレーション)　→　取りあえず動かすなら後回し。
・マーカーの認識　
・pythonでARマーカーの姿勢推定

実装

・コピペで使えるはずです。

ARdetect.py

#!/usr/bin/env python
# -*- coding: utf-8 -*

import numpy as np
import cv2
from cv2 import aruco

def main():
    cap = cv2.VideoCapture(1) #使うカメラによって数値を変更
    # マーカーサイズ
    marker_length = 0.056 # [m]
    # マーカーの辞書選択
    dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

    #camera_matrix = np.load("mtx.npy")
    #distortion_coeff = np.load("dist.npy")
    #カメラキャリブレーションを行った場合、上記を使用。
    camera_matrix = np.array( [[1.42068235e+03,0.00000000e+00,9.49208512e+02],
    [0.00000000e+00,1.37416685e+03,5.39622051e+02],
    [0.00000000e+00,0.00000000e+00,1.00000000e+00]] )
    distortion_coeff = np.array( [1.69926613e-01,-7.40003491e-01,-7.45655262e-03,-1.79442353e-03, 2.46650225e+00] )

    while True:
        ret, img = cap.read()
        corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary)
        aruco.drawDetectedMarkers(img, corners, ids, (0,255,255))

        if len(corners) > 0:
            # マーカーごとに処理
            for i, corner in enumerate(corners):

                rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)

                # 不要なaxisを除去
                tvec = np.squeeze(tvec)
                rvec = np.squeeze(rvec)
                # 回転ベクトルからrodoriguesへ変換
                rvec_matrix = cv2.Rodrigues(rvec)
                rvec_matrix = rvec_matrix[0] # rodoriguesから抜き出し
                # 並進ベクトルの転置
                transpose_tvec = tvec[np.newaxis, :].T
                # 合成
                proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
                # オイラー角への変換
                euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6] # [deg]

                print("ID : " + str(ids[i]))

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

        cv2.imshow('drawDetectedMarkers', img)
        if cv2.waitKey(10) & 0xFF == ord('q'):
            break
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

マーカー　(テスト用)

　生成が面倒な場合にお使いください。

　　　　　　　　　　

※'20 5/12コード内の不要な文字削除
以上、参考になれば。