HoloLensでQRコードの位置と向きを検出する

[追記:2018/6/8]
コメントがあったので追記します。UnProjectVector()のコードは、記事内にもリンクがありますが https://docs.microsoft.com/ja-jp/windows/mixed-reality/locatable-camera に書かれているものとなります。

---

HoloLensでQRコードを読み取るのは Locatable camera in Unity に従ってHoloLensで見ている画像を撮影し、ZXingで読み取ればできます。詳しい手順は HololensでQRコードリーダを作ってみた が参考になると思います。

この記事では、QRコードの読み取りだけでなく空間上でのQRコードの位置と向きの検出を行います。

なお、この記事内での「カメラ」はUnityのカメラではなく、HoloLensで見ている画像を撮影するカメラのことを指します。

ZXing.ResultPoints

ZXingでQRコードを読み取るには、次のようなコードを書きます。

BarcodeReader qrReader = new BarcodeReader();
var qrResult = qrReader.Decode(imageBuffer, imageWidth, imageHeight, BitmapFormat.RGBA32);

このとき qrResult.Text からQRコードの内容が取得できますが、同時に qrResult.ResultPoints からQRコードの３隅にある切り出しシンボルの座標を取得できます（アラインメントパターンもある場合は ResultPoints の4番目以降から取得できますが、今回それは使用しません）。

for (int i = 0; i < 3; ++i) {
    var pixelPos = new Vector2(qrResult.ResultPoints[i].X, qrResult.ResultPoints[i].Y);
    ...
}

切り出しシンボルの座標をHoloLensアプリのワールド座標に変換する

取得された切り出しシンボルの座標は、BarcodeReader.Decode に渡した画像＝HoloLensで撮影した画像内の座標であり、それをHoloLensアプリのワールド座標に変換する必要があります。これは Locatable camera に詳しく書かれていますが、要約すると次のようになります。

• 画像の座標系 [0 ~ width/height] を [-1 ~ 1] の座標系に変換
• カメラの射影変換行列（後述）を使って逆射影変換し、カメラのローカル座標系に変換
• カメラのビュー変換行列（後述）を使ってワールド座標系に変換

しかし、画像として撮影したものから3次元の座標を完全に復元することはできないので、実際に得られるのはワールド座標そのものではなくカメラからの方向になります。したがって、その方向へRayを飛ばすことで座標を得る必要があります。

カメラのビュー変換行列と射影変換行列

冒頭で挙げた記事にもあるように、HoloLensで見ている画像を撮影するコードには次のような関数が必要になります。

void OnCapturedPhotoToMemory(
    PhotoCapture.PhotoCaptureResult result,
    PhotoCaptureFrame photoCaptureFrame)
{
        ...
}

この関数に渡される PhotoCaptureFrame からカメラのビュー変換行列と射影変換行列を取得できます。

Matrix4x4 projectionMat;
photoCaptureFrame.TryGetProjectionMatrix(out projectionMat);

Matrix4x4 cameraToWorldMat;
photoCaptureFrame.TryGetCameraToWorldMatrix(out cameraToWorldMat)

コード

    void OnCapturedPhotoToMemory(PhotoCapture.PhotoCaptureResult result, PhotoCaptureFrame photoCaptureFrame)
    {
#if !UNITY_EDITOR
        if (result.success)
        {
            List<byte> imageBufferList = new List<byte>();
            photoCaptureFrame.CopyRawImageDataIntoBuffer(imageBufferList);

            Resolution cameraResolution = PhotoCapture.SupportedResolutions.OrderByDescending((res) => res.width * res.height).First();
            int imageWidth = cameraResolution.width;
            int imageHeight = cameraResolution.height;

            ZXing.BarcodeReader qrReader = new ZXing.BarcodeReader();
            var qrResult = qrReader.Decode(imageBufferList.ToArray(), imageWidth, imageHeight, ZXing.BitmapFormat.RGBA32);

            if (qrReader == null)
            {
                Debug.Log("error: BarcodeReader.Decode");
                return;
            }

            Debug.Log(qrResult.Text);

            Matrix4x4 projectionMat;
            if (!photoCaptureFrame.TryGetProjectionMatrix(out projectionMat))
            {
                Debug.Log("error: PhotoCaptureFrame.TryGetProjectionMatrix");
                return;
            }

            Matrix4x4 cameraToWorldMat;
            if (!photoCaptureFrame.TryGetCameraToWorldMatrix(out cameraToWorldMat))
            {
                Debug.Log("error: PhotoCaptureFrame.TryGetCameraToWorldMatrix");
                return;
            }

            if (qrResult.ResultPoints.Length < 3)
            {
                Debug.Log("error: too few ResultPoints");
                return;
            }

            Vector3[] points = new Vector3[3];

            for (int i = 0; i < 3; ++i)
            {
                var pixelPos = new Vector2(qrResult.ResultPoints[i].X, qrResult.ResultPoints[i].Y);
                var imagePosZeroToOne = new Vector2(pixelPos.x / imageWidth, 1 - (pixelPos.y / imageHeight));
                var imagePosProjected = (imagePosZeroToOne * 2) - new Vector2(1, 1);    // -1 to 1 space
                var cameraSpacePos = UnProjectVector(projectionMat, new Vector3(imagePosProjected.x, imagePosProjected.y, 1));
                var worldSpaceRayPoint1 = cameraToWorldMat.MultiplyPoint(Vector3.zero);     // camera location in world space
                var worldSpaceRayPoint2 = cameraToWorldMat.MultiplyPoint(cameraSpacePos);   // ray point in world space

                RaycastHit hit;
                if (!Physics.Raycast(worldSpaceRayPoint1, worldSpaceRayPoint2 - worldSpaceRayPoint1, out hit, 5, 1 << 31))
                {
                    Debug.Log("error: Physics.Raycast failed");
                    return;
                }

                points[i] = hit.point;
            }

            var worldTopLeft = points[1];
            var worldTopRight = points[2];
            var worldBottomLeft = points[0];

            var bottomToTop = worldTopLeft - worldBottomLeft;
            var leftToRight = worldTopRight - worldTopLeft;

            transform.forward = bottomToTop;
            transform.position = worldBottomLeft + (bottomToTop + leftToRight) * 0.5f;
        }
#endif
    }