【Unity】ARKitでOpenCVを利用してマーカー認識を行う

この記事は、[Unityゆるふわサマーアドベントカレンダー 2018 23日目](https://qiita.com/splas_boomerang/items/e6619fb1e6dd92fd231f)の記事です。ちょっと急いで書いて雑な記事になってしまったので、後でしっかり推敲して修正しておきます！

#概要
ARKitでOpenCVforUnityを利用してマーカー認識をした際の忘備録です。
目的はARKitとOpenCVを組み合わせてマーカーのワールド座標を計算する事です。

#利用環境

[Unity ARKit Plugin](https://bitbucket.org/Unity-Technologies/unity-arkit-plugin/downloads/)
[OpenCVforUnity](https://assetstore.unity.com/packages/tools/integration/opencv-for-unity-21088)
[MarkerBased AR Exapmle](https://assetstore.unity.com/packages/templates/tutorials/markerbased-ar-example-29678)

#ARKitから画像を取得する
一番手軽な手法がRenderTextureから取得する方法です。
ARKitがカメラからの画像をレンダリングした後で(OnPostRenderのタイミング)Graphics.Blitで「現在の画面」をrenderTextureにします。

```csharp:
void OnPostRender()
{
   Graphics.Blit(null, renderTexture);
}

```
ただしこの方法だとARKitの画像だけでなくUIやオブジェクトのレンダリング結果もろもろ全部込みになってしまうので、以下のようにrenderTextureを得た後にUIなどを写すカメラを手動でレンダリングします。

画像取得するまでの諸々の処理は以下の通りになります。

```csharp:
Graphics.Blit(null, renderTexture);
//対象のカメラのenableをfalseにして手動でレンダリングして
//Graphics.Blitの後にレンダリングされるようにする
otherCamera.Render();
```

```csharp:
using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using UnityEngine;
using UnityEngine.XR.iOS;
using UnityEngine.UI;

public class ARKitCameraImageDetecter : MonoBehaviour {

    RenderTexture arTexture;

    [SerializeField] Camera otherCamera;

    bool _isSessionStart = false;

    Texture2D _tex;

    private void Start()
    {
        Resolution currentResolution = Screen.currentResolution;
        _tex  = new Texture2D(currentResolution.width, currentResolution.height, TextureFormat.RGBA32, false);
        arTexture = new RenderTexture(_tex.width, _tex.height, 24, RenderTextureFormat.ARGB32);
        UnityARSessionNativeInterface.ARFrameUpdatedEvent += FirstFrameUpdate;
        StartCoroutine(GetTextureLoop());
    }


    IEnumerator GetTextureLoop()
    {
        //OnPostRenderと同等
        var wait = new WaitForEndOfFrame();
        while(true){
　　　　　　　// ARのセッションが始まるまで待つ
            if (!_isSessionStart) {
                yield return wait;
                continue;
            }
            else {
                yield return wait;
            }

            // 画面の表示をレンダーテクスチャにBlit
            // ここに全てのカメラのレンダリングが入ってしまうので、
            // ARKitでとる映像以外(UIなど)はカメラを分けて手動レンダリングする
            Graphics.Blit(null, arTexture);
            //RenderTextureをTexture2Dに変換
            var original = RenderTexture.active;
            RenderTexture.active = arTexture;
            _tex.ReadPixels(new UnityEngine.Rect(0, 0, arTexture.width, arTexture.height), 0, 0);
            _tex.Apply();
            RenderTexture.active = original;
            
            // ARKit以外のCameraはrenderTexture取得後に手動でレンダリング
            otherCamera.Render();
        }
    }


    void FirstFrameUpdate(UnityARCamera cam)
    {
        UnityARSessionNativeInterface.ARFrameUpdatedEvent -= FirstFrameUpdate;
        _isSessionStart = true;
    }

}

```
こうして得られたtextureをOpenCV側に渡して処理をしていきます。

#取得したTexture2Dからマーカをトラッキングする
OpenCVforUnityを買ったら使える[MarkerBased AR Exapmle](https://assetstore.unity.com/packages/templates/tutorials/markerbased-ar-example-29678)にWebCameraから取得した画像でマーカートラッキングするサンプル(WebCamTextureMarkerBasedARExample)がありますが、処理の流れはこれを参考にしました。WebCamTextureMarkerBasedARExampleでマーカー認識を試した時の様子が以下の動画にあります。これをARKitから受け取ったTextureに対して行えば良いです。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">OpenCv for Unityでマーカー認識の性能を試してみた。100個近くのマーカーが同時にトラッキング出来て割と高性能だなとおもた。 <a href="https://t.co/YepPzi5UQg">pic.twitter.com/YepPzi5UQg</a></p>&mdash; unagi (@UnagiHuman) <a href="https://twitter.com/UnagiHuman/status/1025027540509843456?ref_src=twsrc%5Etfw">2018年8月2日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

Texture2Dからマーカーのトラッキングまでの手順が以下の通りです。
殆どWebCamTextureMarkerBasedARExampleと同じような処理になります。

```csharp:
    public void Run(Texture2D texture)
    {
       
        ///OpenCV処理初期化
        if (rgbaMat == null)
        {
            rgbaMat = new Mat(texture.height, texture.width, CvType.CV_8UC4);
         
            Utils.texture2DToMat(texture, rgbaMat);

            float width = rgbaMat.width();
            float height = rgbaMat.height();

            float imageSizeScale = 1.0f;
            float widthScale = (float)Screen.width / width;
            float heightScale = (float)Screen.height / height;
            //処理書くの面倒なので、とりあえずPortraitの場合のみ
            if (widthScale < heightScale)
            {
                imageSizeScale = (float)Screen.height / (float)Screen.width;
            }
            else
            {
                
            }

            //set Camera Parameters 
            int max_d = (int)Mathf.Max(width, height);
            double fx = max_d; double fy = max_d;
            double cx = width / 2.0f; double cy = height / 2.0f;

            camMatrix = new Mat(3, 3, CvType.CV_64FC1);
            camMatrix.put(0, 0, fx);
            camMatrix.put(0, 1, 0);
            camMatrix.put(0, 2, cx);
            camMatrix.put(1, 0, 0);
            camMatrix.put(1, 1, fy);
            camMatrix.put(1, 2, cy);
            camMatrix.put(2, 0, 0);
            camMatrix.put(2, 1, 0);
            camMatrix.put(2, 2, 1.0f);
            Debug.Log("camMatrix " + camMatrix.dump());


            //calibration camera
            Size imageSize = new Size(width * imageSizeScale, height * imageSizeScale);
            double apertureWidth = 0;
            double apertureHeight = 0;
            double[] fovx = new double[1];
            double[] fovy = new double[1];
            double[] focalLength = new double[1];
            Point principalPoint = new Point(0, 0);
            double[] aspectratio = new double[1];
            Calib3d.calibrationMatrixValues(camMatrix, imageSize, apertureWidth, apertureHeight, fovx, fovy, focalLength, principalPoint, aspectratio);
            Debug.Log("imageSize " + imageSize.ToString());
            Debug.Log("apertureWidth " + apertureWidth);
            Debug.Log("apertureHeight " + apertureHeight);
            Debug.Log("fovx " + fovx[0]);
            Debug.Log("fovy " + fovy[0]);
            Debug.Log("focalLength " + focalLength[0]);
            Debug.Log("principalPoint " + principalPoint.ToString());
            Debug.Log("aspectratio " + aspectratio[0]); 
            //To convert the difference of the FOV value of the OpenCV and Unity. 
            double fovXScale = (2.0 * Mathf.Atan((float)(imageSize.width / (2.0 * fx)))) / (Mathf.Atan2((float)cx, (float)fx) + Mathf.Atan2((float)(imageSize.width - cx), (float)fx));
            double fovYScale = (2.0 * Mathf.Atan((float)(imageSize.height / (2.0 * fy)))) / (Mathf.Atan2((float)cy, (float)fy) + Mathf.Atan2((float)(imageSize.height - cy), (float)fy));
            distCoeffs = new MatOfDouble(0, 0, 0, 0);

            if (widthScale < heightScale)
            {
                ARCamera.fieldOfView = (float)(fovx[0] * fovXScale);
            }
            else
            {
                ARCamera.fieldOfView = (float)(fovy[0] * fovYScale);
            }

            var markerDesigns = new MarkerDesign[1];
            markerDesigns[0] = markerSetting.markerDesign;
            markerDetector = new MarkerDetector(camMatrix, distCoeffs, markerDesigns);

            invertYM = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(1, -1, 1));
            Debug.Log("invertYM " + invertYM.ToString());

            invertZM = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(1, 1, -1));
            Debug.Log("invertZM " + invertZM.ToString());

        }
        else
        {
            Utils.texture2DToMat(texture, rgbaMat);
        }


        Core.flip(rgbaMat, rgbaMat, 0);

        markerDetector.processFrame(rgbaMat, 1);

        markerSetting.setAllARGameObjectsDisable();

        List<Marker> findMarkers = markerDetector.getFindMarkers();
        for (int i = 0; i < findMarkers.Count; i++)
        {
            Marker marker = findMarkers[i];

            if (marker.id == markerSetting.getMarkerId())
            {
                transformationM = marker.transformation;

                ARM = ARCamera.transform.localToWorldMatrix * invertYM * transformationM * invertZM;

                GameObject ARGameObject = markerSetting.getARGameObject();
                if (ARGameObject != null)
                {

                    ARUtils.SetTransformFromMatrix(ARGameObject.transform, ref ARM);
                    ARGameObject.SetActive(true);
                }
            }
        }
   }
```
<img width="752" alt="スクリーンショット 2018-08-23 23.41.32.png" src="https://qiita-image-store.s3.amazonaws.com/0/116884/86f9871e-8650-b036-c4ba-41b59e2098e0.png">
これで画像からマーカーが検出され、ARObjectsとしてMarkerSettingsオブジェクト以下の階層にマーカーがスポーンされる。ただし、このマーカーの位置の計算はマーカーが1mx1mのサイズだと仮定して計算しているので、これをリアルマーカーのサイズに合わせて補正する必要がある。

その為に、MarkerSettingsのscaleをリアルマーカーが例えば0.1mの場合、scaleを0.1に設定しておく。

#マーカーの座標をワールド座標系に変換する
上の方法で得られた座標はOpenCVARMarkerCameraから見た座標なので、これをOpenCVARMarkerCamera座標->ARKitのCamera座標->ワールド座標に変換する必要が出てくる。

この変換はTransformの親子構造を利用するとお手軽に計算できる。

* ARObjectのワールド座標をARKitCameraの子供に配置したGameObject(markerCulcPosition)のローカル座標に設定
* markerCulcPositionのワールド座標がマーカーのワールド座標となる。

<img width="619" alt="スクリーンショット 2018-08-23 23.51.36.png" src="https://qiita-image-store.s3.amazonaws.com/0/116884/fe21135a-1ab8-1a79-bb99-1fb2dd8e47da.png">