HoloLens2-ResearchMode-Unityに遠距離(LongThrow)のDepthセンサのAPIを追加する

本日は HoloLens2 の技術調査枠です。
HoloLens2-ResearchMode-Unityに遠距離(LongThrow)のDepthセンサのAPIを追加した手順を記事にします。

2021/06/07追記

本記事で行った修正をプルリクエストしたところ、取り込んで頂けました。
現在では以下のオリジナルのリポジトリを利用すると、修正不要で遠距離(LongThrow)のDepthセンサ情報が取得可能です。
github.com

前提条件

以下の記事の続きになります。
bluebirdofoz.hatenablog.com

HoloLens2-ResearchMode-Unity

HoloLens2-ResearchMode-Unity では現在以下のカメラ情報が取得できます。
・近距離(AHAT)のDepthセンサ
・左右の環境認識カメラ
github.com

HoloLens2 の ResearchMode は更に遠距離(LongThrow)のDepthセンサ映像も取得可能です。
今回は HoloLens2-ResearchMode-Unity をカスタマイズして遠距離(LongThrow)のDepthセンサ映像も取得してみます。

プラグインのカスタマイズ

プラグインをカスタマイズするにはフォルダ直下の HL2UnityPlugin.sln を開きます。

遠距離(LongThrow)のDepthセンサの情報は ResearchModeSensorType 型に DEPTH_LONG_THROW で定義されています。
このため、以下の初期化APIを追加することでセンサ情報の取得が可能になります。
・HL2ResearchMode.cpp

クリックで展開

    void HL2ResearchMode::InitializeLongDepthSensor()
    {
        for (auto sensorDescriptor : m_sensorDescriptors)
        {
            if (sensorDescriptor.sensorType == DEPTH_LONG_THROW)
            {
                winrt::check_hresult(m_pSensorDevice->GetSensor(sensorDescriptor.sensorType, &m_longDepthSensor));
                winrt::check_hresult(m_longDepthSensor->QueryInterface(IID_PPV_ARGS(&m_pLongDepthCameraSensor)));
                winrt::check_hresult(m_pLongDepthCameraSensor->GetCameraExtrinsicsMatrix(&m_longDepthCameraPose));
                m_longDepthCameraPoseInvMatrix = XMMatrixInverse(nullptr, XMLoadFloat4x4(&m_longDepthCameraPose));
                break;
            }
        }
    }

次にテクスチャの取得APIを追加します。
遠距離(LongThrow)の深度をグレースケールに変更する際には AHAT と有効距離が異なるため、変換の際の係数を調整します。
更に深度の有効性をチェックする必要がある多ため、シグマバッファの最上位ビットが 1 か否かをチェックしています。
・HL2ResearchMode.cpp

クリックで展開

    void HL2ResearchMode::StartLongDepthSensorLoop()
    {
        if (m_refFrame == nullptr)
        {
            m_refFrame = m_locator.GetDefault().CreateStationaryFrameOfReferenceAtCurrentLocation().CoordinateSystem();
        }

        m_pLongDepthUpdateThread = new std::thread(HL2ResearchMode::LongDepthSensorLoop, this);
    }

    void HL2ResearchMode::LongDepthSensorLoop(HL2ResearchMode* pHL2ResearchMode)
    {
        // prevent starting loop for multiple times
        if (!pHL2ResearchMode->m_longDepthSensorLoopStarted)
        {
            pHL2ResearchMode->m_longDepthSensorLoopStarted = true;
        }
        else {
            return;
        }

        pHL2ResearchMode->m_longDepthSensor->OpenStream();

        try
        {
            while (pHL2ResearchMode->m_longDepthSensorLoopStarted)
            {
                IResearchModeSensorFrame* pDepthSensorFrame = nullptr;
                ResearchModeSensorResolution resolution;
                pHL2ResearchMode->m_longDepthSensor->GetNextBuffer(&pDepthSensorFrame);

                // process sensor frame
                pDepthSensorFrame->GetResolution(&resolution);
                pHL2ResearchMode->m_longDepthResolution = resolution;

                IResearchModeSensorDepthFrame* pDepthFrame = nullptr;
                winrt::check_hresult(pDepthSensorFrame->QueryInterface(IID_PPV_ARGS(&pDepthFrame)));

                size_t outBufferCount = 0;
                const UINT16* pDepth = nullptr;

                const BYTE* pSigma = nullptr;
                pDepthFrame->GetSigmaBuffer(&pSigma, &outBufferCount);
                pDepthFrame->GetBuffer(&pDepth, &outBufferCount);
                pHL2ResearchMode->m_longDepthBufferSize = outBufferCount;

                auto pDepthTexture = std::make_unique<uint8_t[]>(outBufferCount);

                // get tracking transform
                ResearchModeSensorTimestamp timestamp;
                pDepthSensorFrame->GetTimeStamp(&timestamp);

                auto ts = PerceptionTimestampHelper::FromSystemRelativeTargetTime(HundredsOfNanoseconds(checkAndConvertUnsigned(timestamp.HostTicks)));
                auto transToWorld = pHL2ResearchMode->m_locator.TryLocateAtTimestamp(ts, pHL2ResearchMode->m_refFrame);
                if (transToWorld == nullptr)
                {
                    continue;
                }

                for (UINT i = 0; i < resolution.Height; i++)
                {
                    for (UINT j = 0; j < resolution.Width; j++)
                    {
                        auto idx = resolution.Width * i + j;
                        UINT16 depth = pDepth[idx];
                        depth = (pSigma[idx] & 0x80) ? 0 : depth - pHL2ResearchMode->m_depthOffset;

                        // save as grayscale texture pixel into temp buffer
                        if (depth == 0) { pDepthTexture.get()[idx] = 0; }
                        else { pDepthTexture.get()[idx] = (uint8_t)((float)depth / 4000 * 255); }
                    }
                }

                // save data
                {
                    std::lock_guard<std::mutex> l(pHL2ResearchMode->mu);

                    // save raw depth map
                    if (!pHL2ResearchMode->m_longDepthMap)
                    {
                        OutputDebugString(L"Create Space for depth map...\n");
                        pHL2ResearchMode->m_longDepthMap = new UINT16[outBufferCount];
                    }
                    memcpy(pHL2ResearchMode->m_longDepthMap, pDepth, outBufferCount * sizeof(UINT16));

                    // save pre-processed depth map texture (for visualization)
                    if (!pHL2ResearchMode->m_longDepthMapTexture)
                    {
                        OutputDebugString(L"Create Space for depth map texture...\n");
                        pHL2ResearchMode->m_longDepthMapTexture = new UINT8[outBufferCount];
                    }
                    memcpy(pHL2ResearchMode->m_longDepthMapTexture, pDepthTexture.get(), outBufferCount * sizeof(UINT8));
                }

                pHL2ResearchMode->m_longDepthMapTextureUpdated = true;

                pDepthTexture.reset();

                // release space
                if (pDepthFrame) {
                    pDepthFrame->Release();
                }
                if (pDepthSensorFrame)
                {
                    pDepthSensorFrame->Release();
                }

            }
        }
        catch (...) {}
        pHL2ResearchMode->m_longDepthSensor->CloseStream();
        pHL2ResearchMode->m_longDepthSensor->Release();
        pHL2ResearchMode->m_longDepthSensor = nullptr;

    }

以下の公式サンプルの SensorVisualization の SlateCameraRenderer.cpp を参考に画像への変換ロジックを調整しています。
github.com

上記の修正に合わせてその他、バッファの取得APIや利用可能なAPIの登録を追記しました。
詳細は以下の fork してカスタマイズした GitHub プロジェクトの Add_LongThrowAPI ブランチを参照ください。
github.com

コードを修正したらプラグインをビルドします。
設定を[Release][ARM64]に設定し、[ビルド]を実行します。

これでカスタマイズされたプラグインを作成できました。

サンプルプロジェクトのカスタマイズ

次にサンプルプロジェクトをカスタマイズして遠距離(LongThrow)のDepth映像をアプリで確認してみます。

前回と同様に UnitySample のプロジェクトを開きます。

プラグインの差し替え

最初にプラグインを差し替えます。
Assets/Plugins/WSA/ARM64 配下にある以下の2つのファイルを削除します。
・HL2UnityPlugin.dll
・HL2UnityPlugin.winmd

先ほどカスタマイズしたプラグインをコピーして差し替えます。

差し替えた HL2UnityPlugin.dll の Inspector ビューを開き、以下の通り設定して[Apply]します。

IncludePlatforms：WSAPlayerのみ
CPU：ARM64

描画パネルの作成

次に遠距離Depthの映像を描画するパネルをシーンに追加します。
Hierarchy から MixedRealityPlayspace/MainCamera 配下のオブジェクトを開きます。

PreviewPlaneDepth オブジェクトをコピーして遠距離Depthの映像を描画する PreviewPlaneLongDepth オブジェクトを作成しました。
遠距離Depthで取得できる解像度(320x288)に合わせて以下の通り、Scale 値を設定しています。

Scale X:0.096, Y:-0.0864, Z:1.0

スクリプトの編集

Assets/Scripts 配下の ResearchModeVideoStream.cs を編集して遠距離Depthを取得して描画する処理を追加します。

以下の通り、新たに追加した遠距離Depthの初期化/取得APIを使ってテクスチャに映像を描画する処理を追記しました。
HoloLens2 の遠距離Depthの解像度は 320x288 であるため、テクスチャサイズを合わせる必要があることに注意します。
・ResearchModeVideoStream.cs

クリックで展開

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;
using System.Runtime.InteropServices;

#if ENABLE_WINMD_SUPPORT
using HL2UnityPlugin;
#endif

public class ResearchModeVideoStream : MonoBehaviour
{
#if ENABLE_WINMD_SUPPORT
    HL2ResearchMode researchMode;
#endif

    public GameObject depthPreviewPlane = null;
    private Material depthMediaMaterial = null;
    private Texture2D depthMediaTexture = null;
    private byte[] depthFrameData = null;

    public GameObject longDepthPreviewPlane = null;
    private Material longDepthMediaMaterial = null;
    private Texture2D longDepthMediaTexture = null;
    private byte[] longDepthFrameData = null;

    public GameObject LFPreviewPlane = null;
    private Material LFMediaMaterial = null;
    private Texture2D LFMediaTexture = null;
    private byte[] LFFrameData = null;

    public GameObject RFPreviewPlane = null;
    private Material RFMediaMaterial = null;
    private Texture2D RFMediaTexture = null;
    private byte[] RFFrameData = null;


    public GameObject pointCloudRendererGo;
    public Color pointColor = Color.white;
    private PointCloudRenderer pointCloudRenderer;

    void Start()
    {
        depthMediaMaterial = depthPreviewPlane.GetComponent<MeshRenderer>().material;
        depthMediaTexture = new Texture2D(512, 512, TextureFormat.Alpha8, false);
        depthMediaMaterial.mainTexture = depthMediaTexture;

        longDepthMediaMaterial = longDepthPreviewPlane.GetComponent<MeshRenderer>().material;
        longDepthMediaTexture = new Texture2D(320, 288, TextureFormat.Alpha8, false);
        longDepthMediaMaterial.mainTexture = longDepthMediaTexture;

        LFMediaMaterial = LFPreviewPlane.GetComponent<MeshRenderer>().material;
        LFMediaTexture = new Texture2D(640, 480, TextureFormat.Alpha8, false);
        LFMediaMaterial.mainTexture = LFMediaTexture;

        RFMediaMaterial = RFPreviewPlane.GetComponent<MeshRenderer>().material;
        RFMediaTexture = new Texture2D(640, 480, TextureFormat.Alpha8, false);
        RFMediaMaterial.mainTexture = RFMediaTexture;

        pointCloudRenderer = pointCloudRendererGo.GetComponent<PointCloudRenderer>();

#if ENABLE_WINMD_SUPPORT
        IntPtr WorldOriginPtr = UnityEngine.XR.WSA.WorldManager.GetNativeISpatialCoordinateSystemPtr();
        var unityWorldOrigin = Marshal.GetObjectForIUnknown(WorldOriginPtr) as Windows.Perception.Spatial.SpatialCoordinateSystem;

        researchMode = new HL2ResearchMode();
        researchMode.InitializeDepthSensor();
        researchMode.InitializeLongDepthSensor();
        researchMode.InitializeSpatialCamerasFront();

        researchMode.SetReferenceCoordinateSystem(unityWorldOrigin);
        researchMode.SetPointCloudDepthOffset(0);
        Debug.Log("Successfuly initialize sensors");
        researchMode.StartDepthSensorLoop();
        researchMode.StartLongDepthSensorLoop();
        researchMode.StartSpatialCamerasFrontLoop();
#endif
    }

    bool startRealtimePreview = true;
    void LateUpdate()
    {
#if ENABLE_WINMD_SUPPORT
        // update depth map texture
        if (startRealtimePreview && researchMode.DepthMapTextureUpdated())
        {
            byte[] frameTexture = researchMode.GetDepthMapTextureBuffer();
            if (frameTexture.Length > 0)
            {
                if (depthFrameData == null)
                {
                    depthFrameData = frameTexture;
                }
                else
                {
                    System.Buffer.BlockCopy(frameTexture, 0, depthFrameData, 0, depthFrameData.Length);
                }

                depthMediaTexture.LoadRawTextureData(depthFrameData);
                depthMediaTexture.Apply();
            }
        }
        // update long depth map texture
        if (researchMode.LongDepthMapTextureUpdated())
        {
            byte[] frameTexture = researchMode.GetLongDepthMapTextureBuffer();
            if (frameTexture.Length > 0)
            {
                if (longDepthFrameData == null)
                {
                    longDepthFrameData = frameTexture;
                }
                else
                {
                    System.Buffer.BlockCopy(frameTexture, 0, longDepthFrameData, 0, longDepthFrameData.Length);
                }

                longDepthMediaTexture.LoadRawTextureData(longDepthFrameData);
                longDepthMediaTexture.Apply();
            }
        }
        // update LF camera texture
        if (startRealtimePreview && researchMode.LFImageUpdated())
        {
            byte[] frameTexture = researchMode.GetLFCameraBuffer();
            if (frameTexture.Length > 0)
            {
                if (LFFrameData == null)
                {
                    LFFrameData = frameTexture;
                }
                else
                {
                    System.Buffer.BlockCopy(frameTexture, 0, LFFrameData, 0, LFFrameData.Length);
                }

                LFMediaTexture.LoadRawTextureData(LFFrameData);
                LFMediaTexture.Apply();
            }
        }
        // update RF camera texture
        if (startRealtimePreview && researchMode.RFImageUpdated())
        {
            byte[] frameTexture = researchMode.GetRFCameraBuffer();
            if (frameTexture.Length > 0)
            {
                if (RFFrameData == null)
                {
                    RFFrameData = frameTexture;
                }
                else
                {
                    System.Buffer.BlockCopy(frameTexture, 0, RFFrameData, 0, RFFrameData.Length);
                }

                RFMediaTexture.LoadRawTextureData(RFFrameData);
                RFMediaTexture.Apply();
            }
        }

        // Update point cloud
        if (renderPointCloud)
        {
            float[] pointCloud = researchMode.GetPointCloudBuffer();
            if (pointCloud.Length > 0)
            {
                int pointCloudLength = pointCloud.Length / 3;
                Vector3[] pointCloudVector3 = new Vector3[pointCloudLength];
                for (int i = 0; i < pointCloudLength; i++)
                {
                    pointCloudVector3[i] = new Vector3(pointCloud[3 * i], pointCloud[3 * i + 1], pointCloud[3 * i + 2]);
                }
                //Debug.LogError("Point Cloud Size: " + pointCloudVector3.Length.ToString());
                pointCloudRenderer.Render(pointCloudVector3, pointColor);

            }
        }
#endif
    }

    public void TogglePreviewEvent()
    {
        startRealtimePreview = !startRealtimePreview;
    }

    bool renderPointCloud = true;
    public void TogglePointCloudEvent()
    {
        renderPointCloud = !renderPointCloud;
        if (renderPointCloud)
        {
            pointCloudRendererGo.SetActive(true);
        }
        else
        {
            pointCloudRendererGo.SetActive(false);
        }
    }

    public void StopSensorsEvent()
    {
#if ENABLE_WINMD_SUPPORT
        researchMode.StopAllSensorDevice();
#endif
        startRealtimePreview = false;
    }

    private void OnApplicationFocus(bool focus)
    {
        if (!focus) StopSensorsEvent();
    }
}

ResearchModeVideoStream スクリプトがアタッチされている ResearchModeController オブジェクトの Inspector ビューを開きます。
新たに追加された遠距離用のフィールドに先ほど作成した PreviewPlaneLongDepth の参照を設定します。

ビルドと動作確認

これでサンプルプロジェクトのカスタマイズは完了です。
プロジェクトのビルドを行います。

前回記事の手順と同様にビルド設定とマニフェストの編集を行い、プロジェクトを HoloLens2 にインストールします。

HoloLens2 でアプリを起動して確認します。
遠距離Depthの映像が追加で表示されていれば成功です。