Oculus Goを6 DoF化する、たった一つの冴えたやり方

# はじめに
この記事では、3DoF(方向)のみのVR(Virtual Reality)体験を提供することが可能なOculus Goに対して、Intel製のV-SLAM(Visual SLAM)デバイスであるRealSense T265を接続する事により、6DoF(方向及び移動)のVR体験が可能なHMD(Head Mount Display)を実現する方法について説明します。
(※ 実際には、後で説明するようにRealSense SDK 2.0に対して大幅なパッチを当てる必要があるため、ここの情報だけでは直ぐに実現できない点に関しては、ご容赦の程よろしくお願いします。)

![OculusGo_6DoF.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/112880/67ebc467-26a8-c541-ff7a-00ae17e52309.png)


# 事前準備
## 用意する物
- Oculus社製HMD Oculus Go
- Intel RealSense T265
- Android On-The-Go(OTG)ケーブル(e.g., https://www.amazon.co.jp/dp/B005SZQCWM/)
    - 非USB Type-CなOculus GOは通常USBのデバイス側として認識されてしまうのですが、このOTGケーブルを用いる事によってUSBのホスト側として認識させる事が可能です。
- 開発環境構築用のMac(又はWindows PC)

## H/W側の準備
1. Oculus Goの全面に(上図の様に)両面テープでRealSense T265をアタッチします。
2. Oculus Goのmicro USB端子にOTGケーブルを接続し、さらにRealSense T265に付属のケーブルでRealSense T265と接続します。

## S/Wの準備
1. Mac上に最新のAndroid StudioをインストールしてAndroid向けの会果たす環境を準備します。
2. 最新版の[RealSense SDK 2.0](https://software.intel.com/en-us/realsense/sdk)一式をダウンロードします。

# RealSense SDKにおけるAndroidサポート
## RealSense SDK Ver2.18.0以前の世界
基本的にRoot化していないAndroidデイバスでは、RealSense SDKを利用する事は出来ません。
実際、[Build RealSense SDK Samples for Android OS](https://github.com/IntelRealSense/librealsense/blob/v2.18.1/doc/android/AndroidNativeSamples.md)というドキュメントを読むと以下のような記述が有ります。

> ## Instructions
> 1. [Root](https://www.wikihow.tech/Root-Android-Phones) your Android device.
> 2. Download the [Native Development Kit (NDK)](https://developer.android.com/ndk/downloads/index.html) for Linux to your host machine.
> 3. Install [CMake](https://cmake.org/) 3.6.1 or newer.
> 4. Download [ADB](https://developer.android.com/studio/command-line/adb.html) to the host machine by typing `sudo apt-get install adb`.
> 5. Clone the latest [RealSense&trade; SDK 2.0](https://github.com/IntelRealSense/librealsense/releases) to your host machine.
> 6. Change the streaming width and height to 480 and 270... 
> 7. Open Terminal on the host machine, ...
> > Initialize ANDROID_ABI with one of the [supported ABIs](https://developer.android.com/ndk/guides/abis.html#sa) (`armeabi-v7a` for example).
> 8. When compilation done type the following lines to store the binaries at the same location to easily copy them to your Android device. ...
> 9. Connect your Android device to the host machine using USB OTG cable.
> 10. Create new folder and copy the binaries to your Android device using ADB by the following lines:
> ```
adb shell mkdir -p /storage/emulated/legacy/lrs_binaries
adb push . /storage/emulated/legacy/lrs_binaries/
```

> 11. Open ADB Shell and move to Super User mode by the following line:
> ```
adb shell su
```
> 12. Copy the binaries to the internal storage and change their permission to be executables by the following lines:
> ```shell
cp -R /storage/emulated/legacy/lrs_binaries /data/
cd /data/lrs_binaries
chown root:root *
chmod +x *
```

一方、Oculus GoもAndroid OSベースのOSを搭載しており、Root化する方法についてインターネットを調査してみましたが、未だ実現している例は無い様です。

## RealSense SDK Ver2.19.0以降の世界
実は、RealSense SDK 2におけるRealSense T265のサポートはVer2.19.0からです。
それと同時に、RealSense SDK 2の中にAndroid OSにおけるRealSense D4xx系のサポートがマージされている事が確認出来ました。

当初、RealSense T265のAndroidサポートも可能になった、と思って調べたのですが、前年ながら[Intel® RealSense™ SDK 2.0 for Android OS](https://github.com/IntelRealSense/librealsense/blob/master/doc/android.md)を見ると分かるように以下のような記述が有りました。

> The T265 tracking module is not yet supported on Android via librealsense. Support is planned to be added in a future release.

つまり、RealSense SDK Ver2.19.0からサポートされたのは従来の深度デバイスであるRealSense D400系のデバイスだけでした。（まあ、それでも十分実用的には進歩した事なのですが...)

## RealSenseSDKとAndroid OSの相性が悪い理由
RealSense SDKのアーキテクチャ構造を調査すると直ぐに分かる事なのですが、主原因はLinux OSでUSB Device Driverにアクセスする為に利用しているUserlandのライブラリが[libusb](https://libusb.info/)で有る事に起因しています。
libusbでは、Kernellandのデバイスドライバに対してアクセスするために、デバイスファイル(`/dev/*`)を利用しています。Linux系のOSでは通常の手法で有るが、Androidにおいては、USBデバイスへのアクセスはデバイスファイルでは無く、専用のAPIを用いて行う事が要求されます。
Androidにおいても、USBデバイスを接続したときには動的に対応するデバイスファイルが生成されるので、特殊な手順(Root化、SELinuxをOFF, Permissionを設定)を踏む事によってlibusbが利用可能となり、RealSense SDKもLinuxと同じ動作モードで動作可能ではあるのですが、通常のAndroid端末からの利用においては、絶望的と言わざるを得ません。

新しいRealSense SDK Ver2.19.0以降では、この問題となるlibusbを利用しないAndroid OS専用のUVC Backendが独自に実装された事により、この問題を回避する事が可能となりました。このBackendは次のパス辺りに実装されていますので、興味のある方は覗いてみてください。

```
librealsense/src/android/*
librealsense/wrapper/android/*
```

## なぜ、RealSense T265は動作しないのか？
では、なぜRealSense T265の方はAndroid OSをサポートしないのかという事については、ソースコードを見れば直ぐに分かりますが、従来の深度センサー系(D400)とV-SLAM系(T265)では、実質的な処理部分が全く違うソースツリーとなっている、つまり同じSDKでも実質的な処理は、全く別に行われているという事が原因です。

# RealSense T265のAndroidサポートへの道...
## libusbをどうするか？
libusbを利用しないように実装し直す事は、ほぼ全部を解析して書き直す必要があるので、実質的に手が出ません。そのために、なんとかlibusbをデバイスファイルへのアクセスを行わないで動作させる方法を模索します。
色々調査していると、幸運な事にLibusb v1.0.23-rc1から、Android対応に向けた暫定的な実装がマージされていました。
具体的には、USBデバイスがアタッチされたシーケンスにおいて、通常のAndroid OSのマナーで取得したファイルディスクリプタ(fd)を特別な関数でlibusbに渡す事により、libusbを動作せせる事が可能になっています。

```cpp
int API_EXPORTED libusb_wrap_sys_device(libusb_context *ctx, intptr_t sys_dev,
    libusb_device_handle **dev_handle)
    {
```

この関数の`sys_dev`にfdを渡す事で、libusbを動作させる事が可能になります。ただ、限定事項として`libusb_close()`を読んではいけない等の注意事項もあるので、その部分のパッチもSDKに当てる必要がありました。

## T265内のVPU(Intel Movidius Myriad 2)の挙動について
RealSense T265の内部にはV-SLAMの処理を行うプロセッサVPUが入っています。通常のデバイスであれば、VPUのFlash ROM領域にFirmwareが書かれていて、電源ON(USB接続)と同時にV-SLAMとしての処理が起動するものと思っていました。
ところが、実際には次のような様子を観察する事が出来ました。

1. 見知らぬUSB Product ID (0x03E7, 0x8087…)
 - 通常のRealSense deviceは0x8086
 - 最初は0x03E7だけど...
2. 何故か（2度）呼ばれるonPause() & onResume()
 - AndroidのUSB attachはIntent検出 `android.hardware.usb.action.USB_DEVICE_ATTACHED`
 - なので1度はonPause() -> onResume()が呼ばれるはず...
3. Intel® Movidius™ Myriad™ 2 VPU の(不可思議な)起動プロセス
 - Firmwareを（毎回）ダウンロード
 - 突然T265がリブート
4. あら不思議、USB IDが0x8087に^^)/

つまり、分かったことしてはRealSense SDKのビルド時にFirmwareがライブラリに固定されてしまうことでした。そのため、T265とライブラリの不整合は原理的に起きないのですが、T265の機能アップの恩恵を受けるためにはライブラリを含めた再ビルドが必要ということでした。

このために、RealSense T265をUSB接続した後、少ししてもう一度再接続したようなシーケンスが走る事を考慮し、SDKの動作を調整する必要がありました。

## Oculus Goの6DoF化
大きくは上記の2つの修正ですが、他にもlibusbの拡張も含めて幾つかの変更を行う必要がありますが、何とかRealSense T265をAndroid OS(=== Oculus Go)で動作させる事が出来ました。

以上の修正でC++の世界でT265の情報が取れるようになるので、JNIでSDKを拡張します。

```c:pose.c
#include <jni.h>
#include "error.h"
#include "../../../include/librealsense2/rs.h"

JNIEXPORT jfloat JNICALL
Java_com_intel_realsense_librealsense_PoseFrame_nGetX(JNIEnv *env, jclass type, jlong handle) {
    rs2_error *e = NULL;
    rs2_pose pose;
    rs2_pose_frame_get_pose_data((const rs2_frame*) handle, &pose, &e);
    handle_error(env, e);
    return pose.translation.x;
}

JNIEXPORT void JNICALL
Java_com_intel_realsense_librealsense_PoseFrame_nGetPose(JNIEnv *env, jclass type, jlong handle, jfloatArray posXYZ, jfloatArray quaternionXYZW) {
    jfloat* XYZ = (*env)->GetFloatArrayElements(env, posXYZ, NULL);
    jfloat* XYZW = (*env)->GetFloatArrayElements(env, quaternionXYZW, NULL);
    //
    rs2_error *e = NULL;
    rs2_pose pose;
    rs2_pose_frame_get_pose_data((const rs2_frame*) handle, &pose, &e);
    handle_error(env, e);
    //
    XYZ[0] = pose.translation.x;
    XYZ[1] = pose.translation.y;
    XYZ[2] = pose.translation.z;
    XYZW[0] = pose.rotation.x;
    XYZW[1] = pose.rotation.y;
    XYZW[2] = pose.rotation.z;
    XYZW[3] = pose.rotation.w;
    //
    (*env)->ReleaseFloatArrayElements(env, posXYZ, XYZ, 0);
    (*env)->ReleaseFloatArrayElements(env, quaternionXYZW, XYZW, 0);

    return;
}
```

更に、Javaの世界でもSDKを次のように拡張します。

```java:PoseFrame.java
package com.intel.realsense.librealsense;

public class PoseFrame extends Frame {
    public float getX() {
        return nGetX(mHandle);
    }
    public void getPose(float[] posXYZ, float[] quaternionXYZW) { nGetPose(mHandle, posXYZ, quaternionXYZW); }

    protected PoseFrame(long handle) {
        super(handle);
    }

    private static native float nGetX(long handle);
    private static native void nGetPose(long handle, float[] posXYZ, float[] quaternionXYZW);
}
```

最後に、MainActivityに次のような拡張をして、Unityから位置情報が取れるようにします。

```java:MainActivity.java
    Runnable mStreaming = new Runnable() {
        @Override
        public void run() {
            try {
                try(FrameSet frames = mPipeline.waitForFrames(1000)) {
                    PoseFrame pose_frame = (PoseFrame)frames.first(StreamType.POSE);
                    float[] posXYZ = new float[3];
                    float[] quaternionXYZW = new float[4];
                    pose_frame.getPose(posXYZ, quaternionXYZW);
                    pose_frame.close();
                } catch (Exception e) {
                    // Ignore
                }
                mHandler.post(mStreaming);
            }
            catch (Exception e) {
                Log.e(TAG, "streaming, error: " + e.getMessage());
            }
        }
    };
```

Unityの方では、`OVRCameraRig`(Prefab)に対して次のようなC#スクリプトをアタッチする事により6DoF化が完了します。

```cs:GetRealSenseT265.cs
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class GetRealSenseT265 : MonoBehaviour
{
    private AndroidJavaClass m_mainActivity = null;
    // Start is called before the first frame update
    void Start()
    {
        m_mainActivity = new AndroidJavaClass("com.oculus.UnitySample.MainActivity");
    }

    // Update is called once per frame
    void Update()
    {
        float[] posXYZ = m_mainActivity.GetStatic<float[]>("m_posXYZ");
        float[] quaternionXYZW = m_mainActivity.GetStatic<float[]>("m_quaternionXYZW");
        this.gameObject.transform.localPosition = new Vector3(posXYZ[0], posXYZ[1], -posXYZ[2]);
    }
}
```

# 最後に
以上説明した内容を実行する事で、Oculus Goを6DoFに機能拡張して、Oculus Questと同様の機能を持たせる事が可能になります。
以下の動画は、実際のその動作を撮影した動画になります。
[![Free viewpoint 3D Viewer powered by Looking Glass ](https://i9.ytimg.com/vi/TJmLAkr6YX8/mq1.jpg?sqp=CKzf1-gF&rs=AOn4CLAv049jegR12OOe6OV0YFbUKs6BJA)](https://www.youtube.com/watch?v=TJmLAkr6YX8)