HoloLens カメラの画角を可視化する

HoloLens アドベントカレンダー24日目の記事です！

HoloLensのタップ操作により、写真を撮ると指が映りこんでしまい、取り直すことがよくあります。今回は、HoloLensのカメラの画角を3Dマップ上に可視化してみました。

# 開発環境
- Unity 2017.1.2f1
- Visual Studio 2017(15.4.4)
- HoloToolkit-Unity-v1.2017.2.0.unitypackage
- [HoloPointCloud](https://github.com/sacokoala/HoloPointCloud)
- [Point Cloud Free Viewer](https://www.assetstore.unity3d.com/jp/#!/content/19811)

# 実装

こちらの記事[HoloLensで部屋の3Dスキャン試してみた](https://qiita.com/sacokoala/items/8cfc2222afcbdca6356e)をベースに作っていきます。
[HoloPointCloud](https://github.com/sacokoala/HoloPointCloud)をダウンロードしてください。
Unityで開き、Projectビューのsaco->Scenes->PhotoCaptureRawシーンを選択します。
HoloToolkitとPoint Cloud Free ViewerのShaderフォルダのみインポートします。
SpatialMappingのInspectorビューにあるPhotoCaptureRaw.csを次のように編集します。

~~~
using HoloToolkit.Unity.InputModule;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
using UnityEngine.VR.WSA.WebCam;

public class PhotoCaptureRaw : MonoBehaviour, IInputClickHandler
{

    private PhotoCapture photoCaptureObject = null;
    private bool isCapturing = false;

    private int Width;
    private int Height;

    public int lengthOfLineRenderer;


    // Use this for initialization
    void Start()
    {

        InputManager.Instance.PushFallbackInputHandler(gameObject);

    }

    // Update is called once per frame
    void Update()
    {
        //if (Input.GetKeyDown("space"))
        //{
        //    FrameDisplay();
        //}
    }

    void /*FrameDisplay()*/IInputClickHandler.OnInputClicked(InputClickedEventData eventData)
    {
        Debug.Log("Tapped");
        if (!isCapturing)
        {
            if (photoCaptureObject != null)
            {
                photoCaptureObject.Dispose();
                photoCaptureObject = null;
            }
            PhotoCapture.CreateAsync(false, OnPhotoCaptureCreated);
            isCapturing = true;
        }
    }

    void OnPhotoCaptureCreated(PhotoCapture captureObject)
    {
        photoCaptureObject = captureObject;

        CameraParameters c = new CameraParameters();
        Resolution cameraResolution = PhotoCapture.SupportedResolutions.OrderByDescending((res) => res.width * res.height).First();

        c.hologramOpacity = 0.0f;
        c.cameraResolutionWidth = cameraResolution.width;
        c.cameraResolutionHeight = cameraResolution.height;
        c.pixelFormat = CapturePixelFormat.BGRA32;


        Width = cameraResolution.width;
        Height = cameraResolution.height;
        Debug.Log(String.Format("width={0}, height={1}", Width, Height));

        //captureObject.StartPhotoModeAsync(c, OnPhotoModeStarted);
        // Activate the camera
        photoCaptureObject.StartPhotoModeAsync(c, delegate (PhotoCapture.PhotoCaptureResult result) {
            // Take a picture
            photoCaptureObject.TakePhotoAsync(OnCapturedPhotoToMemory);
        });
    }

    void CreateFrame(PhotoCaptureFrame photoCaptureFrame)
    {
        // ------------------------------------------------------
        // make xyxrgb
        //
        int texWidth = Width / 8;
        int texHeight = Height / 8;
        int numPoints = texWidth * texHeight;
        List<Vector3> points = new List<Vector3>();
        List<int> indecies = new List<int>();

        // ------------------------------------------------------
        // pos & rot
        //
        Matrix4x4 cameraToWorldMatrix;
        photoCaptureFrame.TryGetCameraToWorldMatrix(out cameraToWorldMatrix);
        Matrix4x4 worldToCameraMatrix = cameraToWorldMatrix.inverse;

        Matrix4x4 projectionMatrix;
        photoCaptureFrame.TryGetProjectionMatrix(out projectionMatrix);

        // 0 right
        // 1 up
        // 2 forward
        // 3 position

        // Position the canvas object slightly in front
        // of the real world web camera.
#if UNITY_EDITOR
        Vector3 position = cameraToWorldMatrix.GetColumn(3) + cameraToWorldMatrix.GetColumn(2);
#else
        Vector3 position = cameraToWorldMatrix.GetColumn(3) - cameraToWorldMatrix.GetColumn(2);
#endif

        // Rotate the canvas object so that it faces the user.
        Quaternion rotation = Quaternion.LookRotation(cameraToWorldMatrix.GetColumn(2), cameraToWorldMatrix.GetColumn(1));


        // ------------------------------------------------------
        // pos -> ray
        //
        int pointCount = 0;
        {
            int y = texHeight - 1;
            for (int x = texWidth - 1; x >= 0; --x)
            {
                //int i = y * texWidth + x;
                points.Add(new Vector3(((x - 1) - (texWidth / 2)) / 200.0F,
                                                    (y - (texHeight / 2)) / 200.0F, 0.0F));
#if false
                Vector3 point = points[pointCount];
#else
                // SetTRS
                Vector3 translation = position;
                //Vector3 eulerAngles;
                Vector3 scale = new Vector3(1.0F, 1.0F, 1.0F);
                Matrix4x4 m = Matrix4x4.identity;
                //Quaternion rotation = Quaternion.Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);

                m.SetTRS(translation, rotation, scale);
                Vector3 point = m.MultiplyPoint3x4(points[pointCount]);
#endif

                Vector3 front = new Vector3(point.x - cameraToWorldMatrix.GetColumn(3).x,
                                            point.y - cameraToWorldMatrix.GetColumn(3).y,
                                            point.z - cameraToWorldMatrix.GetColumn(3).z);

                RaycastHit hit;

                if (Physics.Raycast(cameraToWorldMatrix.GetColumn(3), front, out hit, 50.0F))
                {
                    points[pointCount] = hit.point;
                    indecies.Add(pointCount);
                    pointCount++;
                }
                else
                {
                    points.RemoveAt(pointCount);
                }
            }
        }

        {
            int x = 0;
            for (int y = texHeight - 2; y >= 1; --y)
            {
                points.Add(new Vector3(((x - 1) - (texWidth / 2)) / 200.0F,
                                    (y - (texHeight / 2)) / 200.0F, 0.0F));
                //int i = y * texWidth + x;
#if false
                Vector3 point = points[pointCount];
#else
                // SetTRS
                Vector3 translation = position;
                //Vector3 eulerAngles;
                Vector3 scale = new Vector3(1.0F, 1.0F, 1.0F);
                Matrix4x4 m = Matrix4x4.identity;
                //Quaternion rotation = Quaternion.Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);

                m.SetTRS(translation, rotation, scale);
                Vector3 point = m.MultiplyPoint3x4(points[pointCount]);
#endif

                Vector3 front = new Vector3(point.x - cameraToWorldMatrix.GetColumn(3).x,
                                            point.y - cameraToWorldMatrix.GetColumn(3).y,
                                            point.z - cameraToWorldMatrix.GetColumn(3).z);

                RaycastHit hit;

                if (Physics.Raycast(cameraToWorldMatrix.GetColumn(3), front, out hit, 50.0F))
                {
                    points[pointCount] = hit.point;
                    indecies.Add(pointCount);
                    pointCount++;
                }
                else
                {
                    points.RemoveAt(pointCount);
                }
            }
        }
        {
            int y = 0;
            for (int x = 0; x <= texWidth - 1; ++x)
            {
                //int i = y * texWidth + x;
                points.Add(new Vector3(((x - 1) - (texWidth / 2)) / 200.0F,
                                    (y - (texHeight / 2)) / 200.0F, 0.0F));
#if false
                Vector3 point = points[pointCount];
#else
                // SetTRS
                Vector3 translation = position;
                //Vector3 eulerAngles;
                Vector3 scale = new Vector3(1.0F, 1.0F, 1.0F);
                Matrix4x4 m = Matrix4x4.identity;
                //Quaternion rotation = Quaternion.Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);

                m.SetTRS(translation, rotation, scale);
                Vector3 point = m.MultiplyPoint3x4(points[pointCount]);
#endif

                Vector3 front = new Vector3(point.x - cameraToWorldMatrix.GetColumn(3).x,
                                            point.y - cameraToWorldMatrix.GetColumn(3).y,
                                            point.z - cameraToWorldMatrix.GetColumn(3).z);

                RaycastHit hit;

                if (Physics.Raycast(cameraToWorldMatrix.GetColumn(3), front, out hit, 50.0F))
                {
                    points[pointCount] = hit.point;
                    indecies.Add(pointCount);
                    pointCount++;
                }
                else
                {
                    points.RemoveAt(pointCount);
                }
            }

        }
        {
            int x = texWidth - 1;
            for (int y = 1; y < texHeight - 1; ++y)
            {
                //int i = y * texWidth + x;
                points.Add(new Vector3(((x - 1) - (texWidth / 2)) / 200.0F,
                                    (y - (texHeight / 2)) / 200.0F, 0.0F));
#if false
                Vector3 point = points[pointCount];
#else
                // SetTRS
                Vector3 translation = position;
                //Vector3 eulerAngles;
                Vector3 scale = new Vector3(1.0F, 1.0F, 1.0F);
                Matrix4x4 m = Matrix4x4.identity;
                //Quaternion rotation = Quaternion.Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);

                m.SetTRS(translation, rotation, scale);
                Vector3 point = m.MultiplyPoint3x4(points[pointCount]);
#endif

                Vector3 front = new Vector3(point.x - cameraToWorldMatrix.GetColumn(3).x,
                                            point.y - cameraToWorldMatrix.GetColumn(3).y,
                                            point.z - cameraToWorldMatrix.GetColumn(3).z);

                RaycastHit hit;

                if (Physics.Raycast(cameraToWorldMatrix.GetColumn(3), front, out hit, 50.0F))
                {
                    points[pointCount] = hit.point;
                    indecies.Add(pointCount);
                    pointCount++;
                }
                else
                {
                    points.RemoveAt(pointCount);
                }
            }
        }

        Debug.Log(pointCount);

        LineRenderer lineRenderer = GetComponent<LineRenderer>();
        lengthOfLineRenderer = pointCount;
        lineRenderer.positionCount = pointCount;

        var t = Time.time;
        for (int i = 0; i < lengthOfLineRenderer; i++)
        {
            lineRenderer.SetPosition(i, points[i]);
        }
    }

    void OnStoppedPhotoMode(PhotoCapture.PhotoCaptureResult result)
    {
        photoCaptureObject.Dispose();
        photoCaptureObject = null;
        isCapturing = false;
    }

    void OnCapturedPhotoToMemory(PhotoCapture.PhotoCaptureResult result, PhotoCaptureFrame photoCaptureFrame)
    {
        if (result.success)
        {
            string filename = string.Format(@"CapturedImage{0}_n.jpg", Time.time);
            string filePath = System.IO.Path.Combine(Application.persistentDataPath, filename);

            photoCaptureObject.TakePhotoAsync(filePath, PhotoCaptureFileOutputFormat.JPG, OnCapturedPhotoToDisk);
            CreateFrame(photoCaptureFrame);
        }
        else
        {
            Debug.LogError("Unable to start photo mode!");
        }
        photoCaptureObject.StopPhotoModeAsync(OnStoppedPhotoMode);
        isCapturing = false;
    }

    void OnCapturedPhotoToDisk(PhotoCapture.PhotoCaptureResult result)
    {
        if (result.success)
        {
            Debug.Log("Saved Photo to disk!");
        }
        else
        {
            Debug.Log("Failed to save Photo to disk");
        }
    }

}

~~~

画像枠に対して、RayCastのHitPointをLineRendererでつないで表示しています。

SpatialMappingにLineRendererを追加し、Widthを0.01に設定します。
<img width="270" alt="PhotoCaptureRaw01.PNG" src="https://qiita-image-store.s3.amazonaws.com/0/63863/e7a63b9d-9bdb-7f4d-bed8-c39a9e361058.png">

いつものようにビルドして、実行してください。
タップすると撮影された範囲の画角が表示されます。

実行動画はこちらです。
[![PhotoCaptureRaw](https://i.ytimg.com/vi/shYZjFHh4wM/2.jpg?time=1515113702861)](https://youtu.be/shYZjFHh4wM)

指が映りこんでいたら音などで知らせてくれるといいかもです。

PhotoCaptureFrameを取得してからでないとカメラのMatrixが得られないので、常に撮影する必要がありますね...

![20180104_171952_HoloLens_Moment.jpg](https://qiita-image-store.s3.amazonaws.com/0/63863/8d873834-e6e2-2314-a0e8-59139a3e9785.jpeg)
Fig.1 MRCで撮影した画像

![CapturedImage122.6867_n.jpg](https://qiita-image-store.s3.amazonaws.com/0/63863/316cb058-6fd6-176b-59d7-0f240054565c.jpeg)
Fig.2 PhotoCaptureで撮影した画像

# 参考記事
- [HoloLensで部屋の3Dスキャン試してみた](https://qiita.com/sacokoala/items/8cfc2222afcbdca6356e)
- [LineRenderer.SetPosition](https://docs.unity3d.com/ScriptReference/LineRenderer.SetPosition.html)