ARKitでKinectの点群を表示してみた

昨年(2017/12)Unityが32bitメッシュに対応したことで、Unityで点群を扱うのが簡単になりました。
これによりプラットフォームを跨いだ利用がしやすくなり、今回Kinect+Unityで撮影した点群を転送し、ARKit+Unityで表示しました。
ここでは主にどういった手法でARKitに実装したかをまとめています。 

# 技術背景

1. Unity2017.3(2017/12)で32bit頂点(約4億頂点)に対応  
   これまでは16bit制限(65,535頂点)で分割して表示する手間があった
2. 32bit対応によりKinectの点群をひとつのメッシュで表示可能となった  
   KinectV2の画素数: 512x424=217,088  

# 動画

[![InteractiveAR](http://img.youtube.com/vi/UftgJDKQyUc/0.jpg)](http://www.youtube.com/watch?v=UftgJDKQyUc)

# 俯瞰図

![top.png](https://qiita-image-store.s3.amazonaws.com/0/35973/64a609d8-cdb7-e773-7034-321732312f79.png)

図のようにKinectで撮影した点群をWebSocketで転送し、ARKitのImageTrackingで表示しています。


# ハードウェア
- KinectV2
- iPad Pro 12.9インチ 第2世代

# 動作確認バージョン
- Win10 1803
- MacOS 10.14
- Unity2018.2.9f1
- Xcode 10.0
  - iOS12.0

# Kinect環境構築

KinectV2をWindowsのUnity上で動かすには、[Kinect for Windows SDK 2.0](https://www.microsoft.com/en-us/download/details.aspx?id=44561)と、[Unity Proパッケージ](https://developer.microsoft.com/ja-jp/windows/kinect)の導入が必要です。
  
- Unity Proパッケージ
  - 解凍し、含まれるKinect.2.0.1410.19000.unitypackageをUnityにImportします。

# Kinect実装

Kinectで撮影した点群を表示します。
コードはGistにアップしています。
https://gist.github.com/otmb/e4eeebeb8bfe7730e892d4ded28b31c7

## コードの説明

32bit Meshを利用するので初めに宣言が必要です。

```Csharp
mesh = new Mesh()
{
    indexFormat = UnityEngine.Rendering.IndexFormat.UInt32,
};
```

イベントハンドラに点群とBodyTrackingのデータを取得するように指示しています。

```Csharp
multiFrameSourceReader = _Sensor.OpenMultiSourceFrameReader(FrameSourceTypes.Depth
    | FrameSourceTypes.Color | FrameSourceTypes.BodyIndex | FrameSourceTypes.Body);

multiFrameSourceReader.MultiSourceFrameArrived += Reader_MultiSourceFrameArrived;
```

イベントにデータが届けばあとは何となくわかります。
重要な箇所としては、マップでしょうか。
MapDepthFrameToColorSpaceでは、「Color座標系をDepth座標系にフレーム単位で変換」
MapDepthFrameToCameraSpaceでは、「Depth座標系をUnityの3D座標に変換」

```Csharp
_Mapper.MapDepthFrameToColorSpace(depthFrameData, colorSpacePoints);
_Mapper.MapDepthFrameToCameraSpace(depthFrameData, cameraSpacePoints);
```

ARKit ImageTrackingの中心に人物を表示したいので人物を検出し、人物の座標からずれた分を補正するようにしました。

```Csharp
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
Body body = bodies.FirstOrDefault(b => b.IsTracked);

var bodyPos = (body == null) ? Vector3.zero : new Vector3(
    body.Joints[JointType.SpineMid].Position.X,
    body.Joints[JointType.SpineMid].Position.Y,
    body.Joints[JointType.SpineMid].Position.Z);

...

vertices[counter] = new Vector3(p.X, p.Y, p.Z) - bodyPos;
```

JointType.SpineMidは骨格検出の背中あたりになります。

![dn758662.skeleton_overview_nui_skeleton(en-us,ieb.10).png](https://qiita-image-store.s3.amazonaws.com/0/35973/62e04812-5a32-10d0-00bf-3cee43d15e17.png)

参照: https://docs.microsoft.com/en-us/previous-versions/windows/kinect/dn758662(v%3dieb.10)


## 転送データの作成

転送データは、点群を固定長のバイナリで渡しました。
コードでは以下を行っています。
1. float配列(単精度)をushort配列(半精度)に置き換え
2. ushort配列からバイト配列に置き換え
3. ushort配列と色のバイト配列と結合したバイト配列を作成

```Csharp
byte[] CreateBinary(float[] vertexs, byte[] colors)
{
    var ushorts = new ushort[vertexs.Length];
    for (int i = 0; i < vertexs.Length; i++)
    {
        ushorts[i] = HalfHelper.SingleToHalf(vertexs[i]);
    }

    var v = GetUshotrToByte(ushorts);
    var buf = v.Concat(colors).ToArray();

    return buf;
}

byte[] GetUshotrToByte(ushort[] ushorts)
{
    var bytes = new byte[ushorts.Length * 2];
    Buffer.BlockCopy(ushorts, 0, bytes, 0, bytes.Length);
    return bytes;
}
```

### データを小さくする

単純にデータを送信すると1データ毎のサイズが大きい為、フレームレートを出すことが出来ません。
また、iPadで毎秒5MB程度受信するとバッファ処理が追い付かなくなり、溜まっていく現象が起きました。
<b>1データを小さくし、受信するフレームを適当な量にするのがよさそうです。</b>

- 単精度から半精度に変換  
単精度から半精度にしたところ、実際の点群の表示に差異を感じなかったので半精度を採用しました。
先程のコードにあるSingleToHalfは[HalfHelper.cs](https://gist.github.com/otmb/a3ede7130031458c0cc98f6428573d1f)を利用しています。
半精度にすることで、1データ3.3MB(217,088x16=3,473,408)が、1.86MB(217,088x9=1,953,792)まで減りました。

![image.png](https://qiita-image-store.s3.amazonaws.com/0/35973/5d0aa62e-3e0e-54bb-195c-951227235db3.png)

- Zstdで圧縮する  
当初LZ4を試しましたが圧縮率が低いのと、秒単位で大量のフレームを処理できないことから、
速度がLZ4より多少遅いが、高圧縮なZstdを選択しました。  
https://gist.github.com/otmb/66a0b40510fd544e3aceaab38192728b  
このコードはUnity(Mac/iOS)にZstdを利用する内容です。

## iOSの受信処理

iPadでは[WebSocketSharp](https://github.com/sta/websocket-sharp)で大きなデータを連続して受信すると稀にソケットのエラーが出ます。
色々試しましたが、エラーを検知することもかなり手間なことから、WebSocketSharpは諦め、ネイティブコードで実装している[Jetfire](https://github.com/acmacalister/jetfire)を利用することにしました。 
Jetfireは[Starscream](https://github.com/daltoniam/starscream)のObj-C版です。

- [Jetfireサンプルコード](https://gist.github.com/otmb/9ad93d1209719731bd60609b9db62c87)

ライブラリを変更することで通信が安定しました。  

## 転送データを復元する

受信したバイナリを座標と色に分け、半精度を単精度に復元します。
これで頂点と色が取得できますので、[KinectView.cs](https://gist.github.com/otmb/e4eeebeb8bfe7730e892d4ded28b31c7)を参考にメッシュを作成、表示が出来ます。

```Csharp

var pLen = buf.Length / 3 * 2;
var points = GetByteToUshort(buf, pLen);

var vertices = new Vector3[points.Length/3];
var colors = new Color[points.Length/3];

for (int i = 0; i < points.Length / 3; i++)
{
    var n = i * 3;
    var x = HalfHelper.HalfToSingle(points[n]);
    var y = HalfHelper.HalfToSingle(points[n+1]);
    var z = HalfHelper.HalfToSingle(points[n+2]);

    var r = buf[pLen + n];
    var g = buf[pLen + n + 1];
    var b = buf[pLen + n + 2];

    vertices[i] = new Vector3(x,y,z);
    colors[i] = new Color32(r,g,b,1);
}

public static ushort[] GetByteToUshort(byte[] bytes,int length)
{
    var ushorts = new ushort[length / 2];
    Buffer.BlockCopy(bytes, 0, ushorts, 0, length);
    return ushorts;
}
```

## UnityARKitPlguinのImageTrackingで点群の表示を実装する

UnityではARKitを実装する為に、[unity-arkit-plugin](https://bitbucket.org/Unity-Technologies/unity-arkit-plugin)をUnityが公開しています。

ここでは、unity-arkit-pluginを導入したとしまして、Unity-ARKit-Plugin/Assets/UnityARKitPlugin/Examples/ARKit1.5/UnityARImageAnchorのフォルダを対象にどのように実装したかをまとめています。

ImageTracking用の表示するオブジェクトの処理は先程のフォルダにあるGenerateImageAnchor.csで行っています。
GenerateImageAnchor.csではXY座標を扱いますが、今回は点群を扱いますので床の位置(Z軸)を取得できるように修正します。

[MyGenerateImageAnchor.cs](https://gist.github.com/otmb/28621781f88dc6a34b35e1edb2740fb2)を用意しましたので参照ください。
MyGenerateImageAnchor.csではHitTestを行うことでZ軸を取得しています。

表示するオブジェクトは、GenerateImageAnchor.csで指定したPrefabが使われます。
Prefabを点群の表示用Prefabに置き換えることで、ImageTrackingで点群を表示します。
点群の表示用に、[PointCloudViewer.cs](https://gist.github.com/otmb/28621781f88dc6a34b35e1edb2740fb2)を用意しました。シェーダーはKinectで利用した[kinectview.shader](https://gist.github.com/otmb/e4eeebeb8bfe7730e892d4ded28b31c7#file-kinectview-shader)を流用できます。
PointCloudViewer.csのメッシュを定期更新することでリアルタイムに点群を更新しています。

# まとめ

今回はKinectを使って、ARKitでリアルタイムに点群を表示する手法についてまとめました。
プロジェクトで書いたコードは試し書きのコードが多く、整理しないとアップできない状態にある為、要点をまとめた内容で情報を公開する運びとしました。

苦労した点だと、通信周りはよくはまります。最初からネイティブのコードで動かしとけばよかった。。

3Dコミュニケーションは未来感がありますが、現段階ではまだまだ粗削りです。
もう少し発展することで今後利用シーンも増えてくるのではないかと考えています。

# おまけ：ブラウザで表示

- [Three.jsの点群処理を参照](https://threejs.org/examples/?q=point#webgl_buffergeometry_points)
- [Zstdをwasmに対応](https://dev.classmethod.jp/client-side/javascript/emscripten-zstandard-on-browser/)
- エンコードはWebWorkerで実装
- [半精度対応](http://stackoverflow.com/questions/6162651/half-precision-floating-point-in-java/6162687)

ブラウザでも受信サイズが大きいとフレームレートが稼げませんでした。
ここでも、圧縮や半精度を使い、データを小さくすることで高速にできました。  

サンプルコード
https://gist.github.com/otmb/a27bf947b08dac1ed8179bf227073192

# 過去作例

以前にマルチカメラでリアルタイムに3Dメッシュの作成に取り組みました。
その時は[PCL(Point Cloud Library)](http://pointclouds.org/)を利用しました。
よければこちらもご覧ください。

- [AppsJapan2017に「バーチャルテレポート」を出展します](https://lab.infocom.co.jp/2017/06/appsjapan2017.html)
- [バーチャルテレポート: メッシュ処理（概要編）](https://lab.infocom.co.jp/2017/06/appsjapan.html)
- [バーチャルテレポート: メッシュ処理（詳細編）](https://lab.infocom.co.jp/2017/06/appsjapan-tips.html)


# References

- [Understanding ARKit Tracking and Detection](https://developer.apple.com/videos/play/wwdc2018/610/)
- [Unity And Kinect V2 Point Cloud](https://gist.github.com/yjiro0403/4ab71d02bc182e3b331cca7c3d5f26e2)  
- [Kinectでユニティちゃんを動かす](https://www.buildinsider.net/small/bookkinectv2/0703)

# Codes

- [KinectV2点群表示](https://gist.github.com/otmb/e4eeebeb8bfe7730e892d4ded28b31c7)
- [HalfHelper.cs(半精度変換)](https://gist.github.com/otmb/a3ede7130031458c0cc98f6428573d1f)
- [Jetfireサンプルコード](https://gist.github.com/otmb/9ad93d1209719731bd60609b9db62c87)
- [Zstd導入](https://gist.github.com/otmb/66a0b40510fd544e3aceaab38192728b)
- [iOS ImageTrackingと点群表示](https://gist.github.com/otmb/28621781f88dc6a34b35e1edb2740fb2)
- [Web PointCloud](https://gist.github.com/otmb/a27bf947b08dac1ed8179bf227073192)