Python + OpenCV ArucoでVRChatでフルトラしようとしたけどうまくいかなかった話

こんにちは、最近VRCにハマりにハマっている者です。
VRCやってるとフルトラしたくなりますよね。でも、フルトラ機器って高いわりにまだまだ発展途上って感じで、買うくらいなら作りたいなってなりませんでしょうか？
作り方は何案かあったんですが、まずはARマーカーベースでやってみようと一日やってみました。OpenCVの経験があるのと、opencv-contribのARマーカー(ArUco)が結構使いやすかった思い出があったためです。
結論から言うとあんまりうまくいきませんでした。
が、同じようなことをしようとする人が居るかもしれないので、メモ代わりに残しておきます。

まず初めに、現状の実行結果を掲載します。所感としては

• ARマーカーをカメラ内に入れておくのが結構大変
• 私の環境では実行速度は20~60FPSで安定しません(@カメラ解像度640x480)
• トラッキング精度が出ずカクカクするが一応足腰動く。

奥行精度が出ないからぶれまくる pic.twitter.com/1vPXqsrxDK

— おぼぁ (@obor1994) February 11, 2023

余談ですが、こちらのアバターは 狛津町仮想商会 様のせせりちゃんです。
手羽が大きくてかわいいので本当に大好きです。僕のトラッカーのせいで妙な動きさせてしまって申し訳ありません。この記事が役に立ったら代わりにせせりちゃんをお買い上げください。
https://booth.pm/ja/items/3360663

手法と比較した特徴

• プリンターさえあればトラッカーを増やせる（ほぼ無料）
• 身に着けるものに充電が不要
• 加速度センサベースのものと比較してドリフト現象が発生しない
• ARマーカー自体は古くからある手法で、画像処理に高度な処理（CNNなど）を必要としないので、動作が軽量なことが期待できる (c.f. mocap for allは動作が重たいらしい(未検証))
• Webカメラ1台でも奥行推定が可能（ただし画像上下左右方向に対して精度は著しく低下）(c.f. mocap for allは2カメラ以上必要)
• ただし、カメラにARマーカーを映さないとトラッキングされない（手やケーブルで隠されるだけでトラッキングが外れる。）

実装

開発言語はPythonで、OpenCVのArUcoを使用します。推定された位置情報をOSC通信で送信し、Virtual Motion TrackerでSteamVRに連携します。
ソースコードは本記事に載せますが、必要なライブラリ（opencv-python, opencv-contrib-python, pyyaml等）はご自身でご準備ください。
ARコード画像の生成 → ChArUcoでのカメラキャリブレーション → Arucoでのフルトラという流れで紹介します。

ARマーカー生成

とりあえず、ArUcoの試しに、指定フォルダにARコードの画像を出力するプログラムです。workdirは環境に合わせて適当なところを指定してください。ArUcoはopencv-pythonだけではなく、opencv-contrib-pythonをインストールしないと使えないのでご注意ください。
ちなみに、ネット検索した感じ見つかったpythonソースコードは軒並み関数名が違いました。最近変わったのかな…？

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

import cv2

workdir = "C:/Users/XXXXXXXXXX/Desktop/arucoFBT/markers/"

aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

for i in range(0,50):
    fileName = "{:0>2}".format(i) + ".png"
    generator = aruco.generateImageMarker(dictionary, i, 500)
    cv2.imwrite(workdir + fileName, generator)

単純にARマーカーを認識させる

出力したARマーカーを認識して認識結果を表示するには以下のソースコードでできます。

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

import cv2
import time

cap = cv2.VideoCapture(0)
#cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) # カメラ画像の横幅を1280に設定
#cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) # カメラ画像の縦幅を720に設定
#cap.set(cv2.CAP_PROP_FPS, 60)           # カメラのフレームレート変更

aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) # 使用するARマーカーの指定
parameters = aruco.DetectorParameters()

start = time.perf_counter()


#繰り返しのためのwhile文
while True:
    #実行速度表示
    print(str(1.0 / (time.perf_counter() - start)) +  "FPS")
    start = time.perf_counter()

    #カメラからの画像取得
    ret, frame = cap.read()

    # ARマーカーを認識
    corners, ids, rejectedCandidates = aruco.detectMarkers(frame, dictionary, parameters=parameters)
    # 認識したARマーカーをカメラ画像に描画
    frame = aruco.drawDetectedMarkers(frame, corners, ids)

    #カメラの画像の出力
    cv2.imshow('camera' , frame)

    #繰り返し分から抜けるためのif文
    key =cv2.waitKey(10)
    if key == 27:
        break

#メモリを解放して終了するためのコマンド
cap.release()
cv2.destroyAllWindows()

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

カメラを使用して位置を測定したりする場合は、カメラの焦点距離や中心位置を記録した行列（cameraMatrix）とレンズ歪み補正用の行列（distCoeffs）を測定しておく必要があります。これはOpenCVではよく、チェスボードを使用しますが、せっかくなのでChArUcoというARマーカー入りのチェスボードを使用します。チェスボードと比較して、チェスボード全体が映らなくてもキャリブレーションに使用できるという利点があり、割と使いやすかったです。
また、ArUcoマーカーを並べただけのボードもキャリブレーションに使えはするのですが、チェスボードと一緒になっているおかげで高精度にボードを認識できるらしいです。

参考：https://docs.opencv.org/3.4/da/d13/tutorial_aruco_calibration.html

以下を実行するとworkdirにチェスボード画像が保存されます。

印刷したチェスボードの大きさを測定し、チェスボードの四角の一辺の大きさをSIZE_OF_CHESSに入れておきましょう。

チェスボードは適当な平たい板に貼りましょう。カメラの前でチェスボードをゆっくり動かします。コンソールに撮影枚数が表示されますので30枚程度とったらEnterキーで終了するとキャリブレーションが実行されます。
キャリブレーション後はチェスボードの姿勢をカメラ画像に表示しますので、カメラの画像とずれがないか確認しましょう。
キャリブレーション結果はyamlファイルで保存します。

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

import cv2
import numpy as np
import yaml

workdir = "C:/Users/XXXXXXXX/Desktop/arucoFBT/"

cap = cv2.VideoCapture(0)
#cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) # カメラ画像の横幅を1280に設定
#cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) # カメラ画像の縦幅を720に設定

aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

# 印刷したボードのチェックマークの1辺
SIZE_OF_CHESS = 29.5 #[mm]

# charucoボード画像を作成し、保存
board = aruco.CharucoBoard((9, 6), 1*SIZE_OF_CHESS, 0.8*SIZE_OF_CHESS, dictionary)
imboard = board.generateImage((1800, 1200))
cv2.imwrite(workdir + "charuco.png", imboard)

parameters = aruco.DetectorParameters()
detector = aruco.CharucoDetector(board)

flgEnter = False
CAPTURE_INTERVAL = 30
intervalCnt = 0

listCharucoCorners = []
listCharucoIds = []

imsize = [0,0]

# ChArUcoボード認識ループ
while True:
    #カメラからの画像取得
    ret, frame = cap.read()
    imsize = frame.shape

    #charucoボードを認識させる
    charucoCorners, charucoIds, markerCorners, markerIds = detector.detectBoard(frame)
    frame = aruco.drawDetectedCornersCharuco(frame, charucoCorners, charucoIds)
    #frame = aruco.drawDetectedMarkers(frame, markerCorners, markerIds)
    
    #カメラの画像の表示
    cv2.imshow('camera' , frame)

    if intervalCnt >= CAPTURE_INTERVAL and not charucoIds is None and len(charucoIds) > 20:
        # 前回の撮影から指定フレーム数が経過 AND 20個以上のマーカーを認識したら
        intervalCnt = 0
        listCharucoCorners.append(charucoCorners)
        listCharucoIds.append(charucoIds)
        cv2.imshow('captured' , frame)
        print("capturedBoard (" + str(len(listCharucoCorners)) + ")")
    elif intervalCnt < CAPTURE_INTERVAL:
        intervalCnt += 1

    #繰り返し文から抜ける
    key =cv2.waitKey(10)
    if key == 27: #Esc
        break
    elif key == 13: #Enter
        flgEnter = True
        break

cv2.destroyAllWindows()

# Enterキーで終了したら撮影データでキャリブレーション
if flgEnter:
    print("startedCalibration...")
    cameraMatrixInit = np.array([[ 1000.,    0., imsize[1]/2.],
                                [    0., 1000., imsize[0]/2.],
                                [    0.,    0.,           1.]])
    distCoeffsInit = np.zeros((5,1))
    
    # キャリブレーション実行そこそこ時間がかかる。
    retval, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(
        listCharucoCorners,
        listCharucoIds,
        board,
        (imsize[1],imsize[0]),
        cameraMatrixInit,
        distCoeffsInit
    )
    print(retval)

    # yaml形式でキャリブレーション結果を保存する
    yml = {
        "cameraMatrix" : cameraMatrix,
        "distCoeffs" : distCoeffs
    }
    print(yml)
    with open(workdir + 'cameraData.yaml', 'w') as file:
        yaml.dump(yml, file)


    #キャリブレーション結果を表示
    while True:
        #カメラからの画像取得
        ret, frame = cap.read()
        imsize = frame.shape

        #charucoボードを認識させて姿勢を推定する
        charucoCorners, charucoIds, markerCorners, markerIds = detector.detectBoard(frame)
        retval, rvec, tvec = aruco.estimatePoseCharucoBoard(charucoCorners, charucoIds, board, cameraMatrix, distCoeffs, rvecs[0], tvecs[0])
        
        #認識したChArUcoボードとボードの姿勢をカメラ画像に描画
        frame = aruco.drawDetectedCornersCharuco(frame, charucoCorners, charucoIds)
        #frame = aruco.drawDetectedMarkers(frame, markerCorners, markerIds)
        frame = cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs, rvec, tvec, 100)

        
        #カメラの画像の表示
        cv2.imshow('camera' , frame)

        #繰り返し文から抜ける
        key =cv2.waitKey(10)
        if key == 27 or key == 13: #Esc or Enter
            break


#メモリを解放して終了
cap.release()
cv2.destroyAllWindows()

ArUcoを使用してフルトラする

最後にARマーカーで測定した位置情報をVMTに送るソースコードを掲載します。
ただし、このソースコードは多分いろいろ間違っています。
まず、回転のつじつまが合っていません。回転の軸があべこべになっていますので、要修正です。回転ベクトルの仕様がopenCVとUnityで違って、見様見真似で変換してみましたが、間違ってるみたいです。。。

なお、このプログラムでは先ほど測定したカメラキャリブレーション結果をyamlファイルから読み込んで使用します。同じworkdirを指定してください。

先ず、ARマーカーを大きく印刷しておいて下さい（id=0,1,2と49を使用します）。印刷したARマーカーの大きさを測定し、MARKER_LENGTHに数値を入れておきます。id=0,1,2は腰、左右の足につけますので、体に取り付けられるようにしてください。私はプラダンに貼り付けて百均のゴム紐で体に巻き付けています。

id=49のARマーカーはホームポジション認識に使用します。プログラム開始後id=49のマーカーを認識するとそこをプレイエリアの中央とします。合わなければエンターキーを押すと再度認識します。

私はHMDにPico4を使用しています。streamingAssistant起動→SteamVR起動→VirtualMotionTracker起動→arucoFBTを起動という順で起動させます。その後、id=49を使用してホームポジションを認識させます。
ARマーカーの姿勢はVMTにそのまま送信されますので、VRChatのCalibrateFBTをすれば使えるようになります。

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

import numpy as np
import cv2
import time
import yaml
from pythonosc import udp_client

workdir = "C:/Users/XXXXXX/Desktop/arucoFBT/"

PORT_VMT_IP_ADDRESS = "127.0.0.1"
PORT_VMT_DRIVER = 39570

MARKER_LENGTH = 108

#ロドリゲスの回転ベクトルからクオータニオンへの変換
def rvec2Quartenion(rvec):
    theta = cv2.norm(rvec)
    return (
        rvec[0]/theta*np.sin(theta/2), 
        rvec[1]/theta*np.sin(theta/2), 
        rvec[2]/theta*np.sin(theta/2), 
        np.cos(theta/2)
    )

# OSCクライアント
client = udp_client.SimpleUDPClient(PORT_VMT_IP_ADDRESS, PORT_VMT_DRIVER)

# Webカメラ
cap = cv2.VideoCapture(0)
#cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) # カメラ画像の横幅を1280に設定
#cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) # カメラ画像の縦幅を720に設定
cap.set(cv2.CAP_PROP_FPS, 60)

aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters()


start = time.perf_counter()

homeTvec = None
homeRvec = None

shiftHome = [0,0]

with open(workdir + 'cameraData.yaml', 'r') as yml:
    config = yaml.load(yml, Loader=yaml.Loader)



#繰り返しのためのwhile文
while True:
    fps = 1.0 / (time.perf_counter() - start)
    start = time.perf_counter()

    #カメラからの画像取得
    ret, frame = cap.read()

    # マーカーを認識し、姿勢を推定する
    corners, ids, rejectedCandidates = aruco.detectMarkers(frame, dictionary, parameters=parameters)
    rvecs, tvecs, objPoints = aruco.estimatePoseSingleMarkers(corners, MARKER_LENGTH, config["cameraMatrix"], config["distCoeffs"])

    # マーカーの認識結果、ホームポジションの座標軸、フレームレートをカメラ画像に描画する
    frame = aruco.drawDetectedMarkers(frame, corners, ids)
    if  not homeTvec is None:        
        frame = cv2.drawFrameAxes(frame, config["cameraMatrix"], config["distCoeffs"], homeRvec, homeTvec, 300)
    cv2.putText(frame, '{:.1f}FPS'.format(fps), (10, 30), cv2.FONT_HERSHEY_PLAIN, 1.5, (0, 255, 0), 1)

    #カメラの画像の出力
    cv2.imshow('camera' , frame)

    if not ids is None:
        # 認識したARマーカーそれぞれについて…
        for id,rvec,tvec in zip(ids, rvecs, tvecs):     
            id = id[0]         
            #print("id:" + str(id) +" tvec:"+str(tvec[0])+","+str(tvec[1])+","+str(tvec[2])+ "rvec:"+str(qvec[0])+","+str(qvec[1])+","+str(qvec[2])+","+str(qvec[3]))

            if id==49:
                # ホームポジション認識
                if homeTvec is None: 
                    homeTvec = tvec
                    homeRvec = rvec
            elif id>=0 and id<10:
                # ボディマーカー認識
                if not homeTvec is None:                 
                    # カメラ座標系からホームポジション中心の座標系に変換する   
                    Rmatrix, Jacob = cv2.Rodrigues(homeRvec)
                    inv_Rmatrix = np.linalg.inv(Rmatrix)                     
                    relative_tvec = np.matmul(inv_Rmatrix , (tvec[0] - homeTvec[0]))
                    relative_rvec = np.matmul(inv_Rmatrix , rvec[0])

                    # openCVのロドリゲス回転ベクトルからクオータニオンに変換
                    relative_qvec = rvec2Quartenion(relative_rvec)    
                    #print("id:" + str(id) +" tvec:"+str(relative_tvec))

                    # mm単位の位置ベクトルをメートル単位に変換してOSCでVMTに送る
                    client.send_message("/VMT/Room/Unity", [
                            int(id+3),           # index
                            int(1),            # enable
                            float(0),          # timeoffset
                            float(relative_tvec[0]/1000 + shiftHome[0]),    # x
                            float(relative_tvec[2]/1000),    # y
                            float(relative_tvec[1]/1000 + shiftHome[1]),    # z
                            float(relative_qvec[0]),    # qx
                            float(relative_qvec[2]),    # qy
                            float(relative_qvec[1]),    # qz
                            float(relative_qvec[3])    # qw
                        ])

    #繰り返し分から抜けるためのif文
    key =cv2.waitKey(1)
    if key == 27: #esc
        break
    elif key == 13: #enter 原点リセット
        homeTvec = None
        homeRvec = None
    elif key == 97: #a 中心位置調整
        shiftHome[1] -= 0.01
    elif key == 115: #s 中心位置調整
        shiftHome[0] += 0.01
    elif key == 100: #d 中心位置調整
        shiftHome[0] -= 0.01
    elif key == 119: #w 中心位置調整
        shiftHome[1] += 0.01
        
    elif key>=0:
        print(key)

#メモリを解放して終了するためのコマンド
cap.release()
cv2.destroyAllWindows()

まとめ

今回はOpenCVのARマーカーでフルトラをやってみました。まだ精度が微妙で、実用には堪えません。今後もうすこし粘ってみようと思います。

改良案としては

• まず回転軸の矛盾を修正する。
• 1点のトラッキングに複数のARマーカーを使用する（ボードが多少隠れてもトラッキング継続できる。測定する点数がふえるので精度が向上する）
• ARマーカーが小さくなると動いたときに認識できなくなりそうなので、OpticalFlowを併用して動きに強くする
• ホームポジションのと調整が意外と大変なので、XY位置はキーボードを押して現在認識されているマーカーを中央に調整する機能があった方がよいかも

を今のところ考えています。
なんだかんだ言って多分現状一番お金をかけずにトラッキングするならこれだと思います。意外と簡単なのでみんなやってみましょう！！！！そしてうまくいったら教えてほしい！！！！！

フルトラは、気合で、できる！！！　
以上！！！！！