見出し画像

[OpenCV plus Unity]WebカメラのみでのARマーカーの姿勢推定してARっぽい表現をする

はじめに

ARCoreとか使うと簡単にAR表示とかできるそうですが、自分はWindowsPCとiPhoneしか持っていないため、ARアプリの開発ができません・・・。
ですが、UnityでOpenCVを使うことができるため、それを使用したAR表示をしたいと思います。
なんかできたってところもちょこちょこあります。

OpenCV Plus Unityとは

UnityでOpenCVが使えるパッケージです。他にも使えるようになるパッケージはありますが、特徴として

  • 無料

  • サポートがない

があります。
基本的に使いたい人は使ってね。責任は持たないけどみたいなスタンスみたいです。

Asset Store
OpenCV plus Unity | Integration | Unity Asset Store

インポートした後はプレイヤー設定から「アンセーフなコードを許可する」みたいなのを設定する必要があります。

カメラ画像を表示する

Webカメラから画像を取得し、Unityに表示させます。

以下コードです。

        WebCamTexture cTex;

        void CheckPhoto()
        {
            Texture2D res;

            if (cTex == null)
                cTex = new WebCamTexture(WebCamTexture.devices[0].depthCameraName, 100, 100);
            // カメラで撮影
            cTex.Play();

            //カメラ画像をOpenCVで扱うMatに変換
            Mat mat = Unity.TextureToMat(this.cTex);
            //MatをTextureに変換
            res = Unity.MatToTexture(mat);
            
            //Textureを適用
            GetComponent<Renderer>().material.mainTexture = res;
        }

WebCamTextureのコンストラクタで指定しているものは

  • カメラデバイス

  • 要求解像度の幅

  • 要求解像度の高さ

です。
でもピッタリ同じ解像度ではなく、一番近い解像度で返されるそうです。

これでこのスクリプトを適用しているオブジェクトに画像が写ります。

まずはキャリブレーション

カメラの内部パラメータを測定するためにキャリブレーションを行います。

キャリブレーションにはチェス盤のような白黒のマーカーが必要になります。「OpenCV キャリブレーション」で検索すると出てくるので、それをPC画面に表示させてキャリブレーションを行いました。

使用する画像の交点は「奇数×偶数」か「偶数×奇数」にする必要があるそうです。

以下コードです。


        void Calibrate()
        {
            cTex.Play();
            Mat mat = Unity.TextureToMat(this.cTex);

            Point2f[] corners;

            bool isOK;

            isOK = Cv2.FindChessboardCorners(mat, new Size(9, 6), out corners);
            Cv2.DrawChessboardCorners(mat, new Size(9, 6), corners, isOK);

            cameraInfo.cameraMat = new double[3,3];
            cameraInfo.distCoeffs = new double[8];
            Vec3d[] rvecs;
            Vec3d[] tvecs;

            List<Point3f> ObjectPoint = new List<Point3f>(new Point3f[54]);
            int index = 0;
            for(int Pointy = 0; Pointy < 12; Pointy += 2)
            {
                for(int Pointx = 0; Pointx < 18; Pointx += 2)
                {
                    ObjectPoint[index++] = new Point3f(Pointx, Pointy, 0);
                }
            }

            if (isOK)
                res = Cv2.CalibrateCamera
                (
                    new List<List<Point3f>>
                    {
                        ObjectPoint
                    },
                    new List<Point2f[]>{corners},
                    new Size(cTex.width, cTex.height),
                    cameraMat,
                    distCoeffs,
                    out rvecs,
                    out tvecs
                );
        }

cameraInfoにはcameraMatとdistCoeffsの配列があります。
それぞれにカメラの内部パラメータ行列と歪み係数が入ります。
(なぜか歪み係数は0だったため歪みはないものとしています。)
これらの値はズーム等をしない状態であれば不変のため、測定後はデータを保存しておいてください。

今回は9×6の交点が存在する画像でキャリブレーションを行いました。

本来は何枚かの画像を使用するそうですが、1枚のみで行っています。

参考程度に自分のカメラ行列を乗せます。

0,0 = 336.986
0,1 = 0
0,2 = 313.21
1,0 = 0
1,1 = 335.619
1,2 = 183.435
2,0 = 0
2,1 = 0
2,2 = 1

ARマーカーを探して姿勢情報を得る

ARマーカーとして使える画像はデモファイルの中に入っているのでそれを使いました。

以下コードです。


        void Find()
        {
            Texture2D res;
            cTex.Play();
            Mat mat = Unity.TextureToMat(this.cTex);
            // Create default parameres for detection
            DetectorParameters detectorParameters = DetectorParameters.Create();

            // Dictionary holds set of all available markers
            Dictionary dictionary = CvAruco.GetPredefinedDictionary (PredefinedDictionaryName.Dict6X6_250);

            // Variables to hold results
            Point2f[][] corners;
            int[] ids;
            Point2f[][] rejectedImgPoints;

            // Convert image to grasyscale
            Mat grayMat = new Mat ();
            Cv2.CvtColor (mat, grayMat, ColorConversionCodes.BGR2GRAY); 

            // Detect and draw markers
            CvAruco.DetectMarkers (grayMat, dictionary, out corners, out ids, detectorParameters, out rejectedImgPoints);
            CvAruco.DrawDetectedMarkers (mat, corners, ids);

            // Create Unity output texture with detected markers
            if (ids.Length == 0)
            {
                res = Unity.MatToTexture(mat);
                GetComponent<Renderer>().material.mainTexture = res;
                return;
            }

            Vec3d[] rvecs;
            Vec3d[] tvecs;
            Cv2.CalibrateCamera
            (
                    new List<List<Point3f>>
                    {
                        new List<Point3f>
                        {
                            new Point3f(-1.5f , -1.5f , 0),
                            new Point3f(1.5f  , -1.5f , 0),
                            new Point3f(1.5f  , 1.5f  , 0),
                            new Point3f(-1.5f , 1.5f  , 0),
                        }
                    },
                    corners,
                    new Size(cTex.width, cTex.height),
                    cameraInfo.cameraMat,
                    cameraInfo.distCoeffs,
                    out rvecs,
                    out tvecs,
                    CalibrationFlags.UseIntrinsicGuess
            );
        }

今回カメラの内部パラメーターは先ほど測定したものを使用するので、 CalibrationFlags.UseIntrinsicGuessのフラグを渡すようにしています。

これでrvecsには回転に関する情報、tvecsに位置に関する情報が手に入ります。

tvecsの情報を変換する

その前にUntiyのオブジェクトの配置関係ですが、カメラの前方(Z軸)100の位置に平面を配置し、そこにカメラ画像を映しています。

また、ARマーカー上に置くオブジェクトのTransformをmarkerObj、カメラの位置のTransformをcameraObjとしています。

以下コードです。

            Vector3 tvecsV3 = new Vector3((float)tvecs[0].Item0, -(float)tvecs[0].Item1, (float)tvecs[0].Item2);
            // z軸(前後方向の距離)の情報を保存
            float distance = (float)tvecs[0].Item2;

            // 位置情報をz = 100の位置になるまで拡大
            float disz = 100 / tvecsV3.z;
            tvecsV3 *= disz;

            // カメラからの相対位置を考えて配置
            markerObj.position = tvecsV3 + cameraObj.position;

            // ※スケールの処理
            float scale = 15f * 2.7f / (distance - 1.15f);
            markerObj.localScale = new Vector3(scale, scale, scale);

大体は書いてある通りです

スケールの処理について

画像のオブジェクトの大きさは距離によって反比例します。
今回はカメラとマーカーの距離とその時のtvecs[0].Item2の値から式を作っています。

それぞれの値については

15 : それっぽい大きさになるように入れた係数
2.7f : 10cmごとのtvecs[0].Item2の変化
1.15 : 距離が0cmでも0にならないのでオフセット調整

こんな関係です。

rvecsの情報を変換する

ここに入れられる値はロドリゲスとかいう値だそうです。
これはロドリゲスの回転公式から来ている名前みたいですが、それぞれの値が何を表しているかについては詳しく書かれていませんでした。

今回はこれを回転行列に変換し、それをクォータニオンに変換するようにします。

以下コードです

            // OpenCVの関数で回転行列を作成
            double[,] rotateMat = new double[3,3];
            // Y軸の回転が逆になっているので第2変数を反転。
            Cv2.Rodrigues(new double[]{rvecs[0].Item0, -rvecs[0].Item1, rvecs[0].Item2}, out rotateMat);

            Quaternion rotateQ;
            // 回転行列からクォータニオンを作成
            Utils.MatToQua(rotateMat, out rotateQ);
            // カメラの傾きが出てくるのでその逆回転を与える
            markerObj.rotation = Quaternion.Inverse(rotateQ);

第2変数を反転することで回転が逆になる理由は分かってないです・・・。
また調べてみます。

Utils.MatToQuaは自作関数になります。
以下コードです。


    public static bool MatToQua(float[,] mat, out Quaternion res)
    {
        if (mat.Length != 9)
        {
            Debug.Assert(true, "This matrix is invalid");
            res = Quaternion.identity;
            return false;
        }
        // 最大成分を検索
        float[] elem = new float[4]; // 0:x, 1:y, 2:z, 3:w
        elem[ 0 ] = mat[0,0] - mat[1,1] - mat[2,2] + 1.0f;
        elem[ 1 ] = -mat[0,0] + mat[1,1] - mat[2,2] + 1.0f;
        elem[ 2 ] = -mat[0,0] - mat[1,1] + mat[2,2] + 1.0f;
        elem[ 3 ] = mat[0,0] + mat[1,1] + mat[2,2] + 1.0f;

        int biggestIndex = 0;
        for ( int i = 1; i < 4; i++ )
        {
            if ( elem[i] > elem[biggestIndex] )
                biggestIndex = i;
        }

        if ( elem[biggestIndex] < 0.0f )
        {
            Debug.Assert(true, "This matrix is invalid");
            res = Quaternion.identity;
            return false;
        }

        // 最大要素の値を算出
        float[] quatanion = new float[4];
        float v = Mathf.Sqrt( elem[biggestIndex] ) * 0.5f;
        quatanion[biggestIndex] = v;
        float mult = 0.25f / v;

        switch (biggestIndex)
        {
            case 0: // x
                quatanion[1] = (mat[1,0] + mat[0,1]) * mult;
                quatanion[2] = (mat[0,2] + mat[2,0]) * mult;
                quatanion[3] = (mat[2,1] - mat[1,2]) * mult;
                break;
            case 1: // y
                quatanion[0] = (mat[1,0] + mat[0,1]) * mult;
                quatanion[2] = (mat[2,1] + mat[1,2]) * mult;
                quatanion[3] = (mat[0,2] - mat[2,0]) * mult;
                break;
            case 2: // z
                quatanion[0] = (mat[0,2] + mat[2,0]) * mult;
                quatanion[1] = (mat[2,1] + mat[1,2]) * mult;
                quatanion[3] = (mat[1,0] - mat[0,1]) * mult;
                break;
            case 3: // w
                quatanion[0] = (mat[2,1] - mat[1,2]) * mult;
                quatanion[1] = (mat[0,2] - mat[2,0]) * mult;
                quatanion[2] = (mat[1,0] - mat[0,1]) * mult;
                break;
        }

        res.x = quatanion[0];
        res.y = quatanion[1];
        res.z = quatanion[2];
        res.w = quatanion[3];
        return true;
    }

こちらを参考にしています。
その58 やっぱり欲しい回転行列⇔クォータニオン相互変換 (sakura.ne.jp)

変換を確認する

求めた位置・回転でキューブを置いて出力させてみます。
キューブの中心がカメラ画像を通るため、少しめり込むような形にはなります
(以降キューブをマーカーオブジェクトとします)

正面から見ても特に問題はありませんでしたが、斜めからだと違和感のある表示になっています。

原因としてはカメラ画像の平面は回転させず、オブジェクトのみ回転しているため、不自然な見た目になっています。

シェーダーをつくる

この問題を解決するためにシェーダーを変更しました。

カメラ画像のシェ―ダー

UnlitシェーダーのSubShaderに以下を追加します

        ZTest Always
        ZWrite off
        Tags { "RenderType"="Opaque" "Queue" = "Geometry-400"}

前後関係を無視して、自身より後ろにあるオブジェクトも表示させます。

マーカーオブジェクトの子に配置するシェーダー

マーカーオブジェクトの子に平面を配置し、そのシェーダーを書きます。
用途としてはマーカーオブジェクトの傾きによってその床面(カメラ画像側)も同じように回転する必要があります。もちろんその床は透明である必要があるため、追加します。

以下SubShaderに追加する内容です。


        ColorMask 0
        Tags {"RenderType" = "Transparent" "Queue" = "Geometry-500"}

オブジェクトによる色の書き込みを抑え、表示順はカメラ画像より先です。

シェーダー変更後

先ほどよりはかなりマシに見えます。
gif画像で見てみるとかなりARっぽい表示になっています。

最後に

若干の違和感はありますが、かなりARのような表現ができたと思います。
キャリブレーションをしっかり行ったりすることでもう少し良い結果が得られるでしょうか・・・?
ちなみに二つ以上のARマーカがあるとうまく表示できないと思います。
また、ロドリゲスについてももう少し調べてみます。

追記

CameraInfoクラスのコードになります。
(コードを編集している可能性があるのでエラーがでるかもしれません…)

public class CameraInfo : ScriptableObject
{
    public double[,] cameraMat
    {
        get
        {
            double[,] res = new double[3,3];
            res[0,0] = cameraMat00;
            res[0,1] = cameraMat01;
            res[0,2] = cameraMat02;
            res[1,0] = cameraMat10;
            res[1,1] = cameraMat11;
            res[1,2] = cameraMat12;
            res[2,0] = cameraMat20;
            res[2,1] = cameraMat21;
            res[2,2] = cameraMat22;
            return res;
        }
        set
        {
            cameraMat00 = value[0,0];
            cameraMat01 = value[0,1];
            cameraMat02 = value[0,2];
            cameraMat10 = value[1,0];
            cameraMat11 = value[1,1];
            cameraMat12 = value[1,2];
            cameraMat20 = value[2,0];
            cameraMat21 = value[2,1];
            cameraMat22 = value[2,2];
        }
    }

    [SerializeField]
    double cameraMat00;
    [SerializeField]
    double cameraMat01;
    [SerializeField]
    double cameraMat02;
    [SerializeField]
    double cameraMat10;
    [SerializeField]
    double cameraMat11;
    [SerializeField]
    double cameraMat12;
    [SerializeField]
    double cameraMat20;
    [SerializeField]
    double cameraMat21;
    [SerializeField]
    double cameraMat22;

    public double[] distCoeffs
    {
        get
        {
            double[] res = new double[8];
            res[0] = distCoeffs0;
            res[1] = distCoeffs1;
            res[2] = distCoeffs2;
            res[3] = distCoeffs3;
            res[4] = distCoeffs4;
            res[5] = distCoeffs5;
            res[6] = distCoeffs6;
            res[7] = distCoeffs7;
            return res;
        }
        set
        {
            distCoeffs0 = value[0];
            distCoeffs1 = value[1];
            distCoeffs2 = value[2];
            distCoeffs3 = value[3];
            distCoeffs4 = value[4];
            distCoeffs5 = value[5];
            distCoeffs6 = value[6];
            distCoeffs7 = value[7];
        }
    }

    [SerializeField]
    double distCoeffs0;
    [SerializeField]
    double distCoeffs1;
    [SerializeField]
    double distCoeffs2;
    [SerializeField]
    double distCoeffs3;
    [SerializeField]
    double distCoeffs4;
    [SerializeField]
    double distCoeffs5;
    [SerializeField]
    double distCoeffs6;
    [SerializeField]
    double distCoeffs7;

    public void Show()
    {
        string str = "";
        foreach(double num in distCoeffs)
        {
            str += num + ", ";
        }
        Debug.Log
        (
            cameraMat[0,0].ToString("000.000") + ", " + 
            cameraMat[0,1].ToString("000.000") + ", " + 
            cameraMat[0,2].ToString("000.000") + "\n" + 
            cameraMat[1,0].ToString("000.000") + ", " + 
            cameraMat[1,1].ToString("000.000") + ", " + 
            cameraMat[1,2].ToString("000.000") + "\n" + 
            cameraMat[2,0].ToString("000.000") + ", " + 
            cameraMat[2,1].ToString("000.000") + ", " + 
            cameraMat[2,2].ToString("000.000") + "\n" + 
            str
        );
    }
}

この記事が気に入ったらサポートをしてみませんか?