libRealSense と Unity で PointCloud してみるまで

[Intel RealSense Depth Camera D415](https://www.switch-science.com/catalog/3632/) が手に入ったので、予定を変更して(なんの?)、これを使って Unity で PointCloud を表示させるまでの流れを記す。


- 開発環境 
    - Windows 10 Home
    - Visual Studio Community 2017
    - Unity 2017.3.1.f1 Personal
    - librealsense v2.10.0

筆者の事前知識は、
*「2年前に、Kinect v2と Unity でちょっとした研究のようなことを1年間ほどやったことがある」*
というくらいだ。
C# & Unity の経験もそのくらいで(しかも忘れかけてる)、 RealSense は今回はじめて触る。

# librealsense

どうも古いSDKと今のものとはまったく別物らしく、ネットで引っかかる情報がほとんど役に立たない。

最新のコードは [Github](https://github.com/IntelRealSense/librealsense) で公開されていて、Release版のソースコード、SDK、ツール類が**[ここ](https://github.com/IntelRealSense/librealsense/releases)**からダウンロードできる。

私は全部ダウンロードしたが、結局ツール類はSDKをインストールしたら全部入っていた。

もっと言うと、SDKもインストール必須というものではなかった。
デバイスは(少なくともWindowsでは)、繋いだだけですぐ使えるようになる。
今のところ、自分でビルドしなくても dll が手に入る、という程度の意味しかない。


# Unity サンプルを動かしてみる

[Unity Wrapper for RealSense SDK 2.0](https://github.com/IntelRealSense/librealsense/tree/development/wrappers/unity)

![realsenseunity](https://user-images.githubusercontent.com/22654243/35569320-93610f10-05d4-11e8-9237-23432532ad87.png)

上のリンク先にも書いてあるが、一応簡単に手順を述べておく。

dll は自分でビルドする方法と、SDKから取ってくる方法がある。
ビルドは面倒なので後者を選択する。

1. [Intel.RealSense.SDK.exe](https://github.com/IntelRealSense/librealsense/releases) をダウンロードしてインストールする。
2. [Github](https://github.com/IntelRealSense/librealsense) からソースコードをダウンロードする。
3. Unity で新しいプロジェクトを作り、ソースコードの `wrappers/unity` 以下をプロジェクトにコピーする。
4.  `"{SDKインストールフォルダ}\bin\x64\LibrealsenseWrapper.dll"` を、Unityプロジェクト内の `Assets/Plugin.Managed/` 以下に配置する。
5.  `"{SDKインストールフォルダ}\bin\x64\realsense2.dll"`を、Unityプロジェクト内の `Assets/RealSenseSDK2.0/Plugins/` 以下に配置する。

これで動くはずだが、私は動かなかった。
"no device found" とあってジタバタしたが、結局、 Windows 再起動してつなぎ直したら認識した。

<br>
ここまではよかった。

# やはりデプスセンサーと言えばPointCloudでしょ

ここまで、APIについてまったく調べていない。
Unity のサンプルにPointCloudはなかった。
C++のサンプルコードはあった。`rs2::pointcloud` クラスを使っていた。

*「じゃあ対応するC#クラスを同じように使えばいいのね」*


C# でAPIの一覧を見るには**[オブジェクトブラウザ](http://shirakamisauto.hatenablog.com/entry/2015/06/29/144514)**を使えばいいらしい。
それっぽいクラスを探してみる。

![2018-02-25-1.png](https://qiita-image-store.s3.amazonaws.com/0/140877/13880afd-2d1a-218c-77aa-c8b41f2d8674.png)

ない。

どうやら本当にないっぽい。
どうすんだ?

## 自分で PointCloud ラッパーを作る

LibrealsenseWrapper.dll をいじるしかない、という結論になった。
そこで、 `"librealsense-2.10.0\wrappers\csharp\Intel.RealSense.SDK.sln"`を開いてみる。


・・・なんか、手抜き感がある。エラーコード読み捨ててるし。
wrapper ライブラリはそれ自体がサンプルコードレベルなのかもしれない。

`rs::pointcloud` と C# の他のコードを見比べつつ、C#版 `PointCloud`クラスと `Points` クラスをつくる。
この段階にいたってもまだAPI機能については調べていない。
まわりのコードの空気を読んで移植を行う。

```c#:PointCloud.cs
using System;
using System.Runtime.InteropServices;
using System.Linq;

namespace Intel.RealSense
{
    public class Points : Frame
    {
        public Points(IntPtr ptr) : base(ptr)
        {
        }
        public int Size
        {
            get
            {
                object error;
                var c = NativeMethods.rs2_get_frame_points_count(m_instance.Handle, out error);
                return c;
            }
        }
        public IntPtr Vertices
        {
            get
            {
                object error;
                var v = NativeMethods.rs2_get_frame_vertices(m_instance.Handle, out error);
                return v;
            }
        }
        public IntPtr TextureCoordinates
        {
            get
            {
                object error;
                var v = NativeMethods.rs2_get_frame_texture_coordinates(m_instance.Handle, out error);
                return v;
            }
        }
    }
    public class PointCloud : ProcessingBlock
    {
        public PointCloud()
        {
            object error;
            m_instance = new HandleRef(this, NativeMethods.rs2_create_pointcloud(out error));
            queue = new FrameQueue();
            NativeMethods.rs2_start_processing_queue(m_instance.Handle, queue.m_instance.Handle, out error);
        }
        public Points Calclate(DepthFrame original)
        {
            object error;
            NativeMethods.rs2_frame_add_ref(original.m_instance.Handle, out error);
            NativeMethods.rs2_process_frame(m_instance.Handle, original.m_instance.Handle, out error);
            return queue.WaitForFrame() as Points;
        }
        public void MapTo(Frame mapped)
        {
            this.Options.Where(s => s.Key == Option.TextureSource).First().Value = mapped.Profile.UniqueID;
            object error;
            NativeMethods.rs2_frame_add_ref(mapped.m_instance.Handle, out error);
            NativeMethods.rs2_process_frame(m_instance.Handle, mapped.m_instance.Handle, out error);
        }

        FrameQueue queue;
    }
}
```

もう一つ。FrameSet.cs を修正する必要があった。

```c#:FrameSet.cs

        internal static Frame CreateFrame(IntPtr ptr)
        {
            object error;
            if (NativeMethods.rs2_is_frame_extendable_to(ptr, Extension.DepthFrame, out error) > 0)
                return new DepthFrame(ptr);
            else if (NativeMethods.rs2_is_frame_extendable_to(ptr, Extension.VideoFrame, out error) > 0)
                return new VideoFrame(ptr);

            // 以下の2行を追加する。
            else if (NativeMethods.rs2_is_frame_extendable_to(ptr, Extension.Points, out error) > 0)
                return new Points(ptr);

            else
                return new Frame(ptr);
        }
```

終わったらビルドして、出来上がった LibrealsenseWrapper.dll を Unity プロジェクトに持っていく。


## Unity で PointCloudViewer をつくる

これで Unity から PointCloud が使えるようになった。

私の苦労話なんかは誰も興味ないだろうから、とりあえず最低限動くコードを載せておく。

*2018/02/28 追記 後の伏線も兼ねて少しコードを修正*
*2019/07/10 追記 65536インデックス制限がなくなったのでそれに合わせて修正*

```c#:PointCloudViewer2.cs
using UnityEngine;
using System.Linq;
using System.Threading;
using Intel.RealSense;

public class PointCloudViewer2 : MonoBehaviour
{
    public Material material;
    AutoResetEvent f = new AutoResetEvent(false);
    PointCloud pointCloud = new PointCloud();
    Texture2D texture;
    Mesh mesh;
    byte[] image;
    Vector3[] vertices;
    Vector2[] texcoords;

    void Start ()
    {
        var colorProfile = RealSenseDevice.Instance.ActiveProfile.Streams
            .First(p => p.Stream == Stream.Color) as VideoStreamProfile;
        if (colorProfile == null) return;
        texture = new Texture2D(colorProfile.Width, colorProfile.Height, TextureFormat.RGB24, false, true)
        {
            wrapMode = TextureWrapMode.Clamp,
            filterMode = FilterMode.Point
        };
        if (material == null)
        {
            material = new Material(Shader.Find("Unlit/Texture"));
        }
        material.mainTexture = texture;
        gameObject.AddComponent<MeshRenderer>().material = material;
        RealSenseDevice.Instance.onNewSampleSet += OnFrameSet;
    }
    [System.Runtime.InteropServices.DllImport("kernel32.dll", EntryPoint = "RtlCopyMemory")]
    static unsafe extern void NativeCopyMemory(void* Destination, void* Source, uint Length);

    private unsafe void OnFrameSet(FrameSet frames)
    {
        var color = frames.ColorFrame;
        var points = pointCloud.Calclate(frames.DepthFrame);
        pointCloud.MapTo(color);
        if (image == null)
        {
            image = new byte[color.Stride * color.Height];
            vertices = new Vector3[points.Size];
            texcoords = new Vector2[points.Size];
        }
        color.CopyTo(image);
        fixed (Vector3* vd = &vertices[0])
        fixed (Vector2* td = &texcoords[0])
        {
            Vector3* vs = (Vector3*)points.Vertices;
            Vector2* ts = (Vector2*)points.TextureCoordinates;
            NativeCopyMemory(vd, vs, (uint)(sizeof(Vector3) * vertices.Length));
            NativeCopyMemory(td, ts, (uint)(sizeof(Vector2) * texcoords.Length));
        }
        f.Set();
    }
    void Update()
    {
        if (f.WaitOne(0))
        {
            if (mesh == null)
            {
                mesh = new Mesh
                {
                    indexFormat = UnityEngine.Rendering.IndexFormat.UInt32,
                    vertices = vertices,
                    uv = texcoords
                };
                mesh.SetIndices(Enumerable.Range(0, vertices.Length).ToArray(), MeshTopology.Points, 0);
                gameObject.AddComponent<MeshFilter>().sharedMesh = mesh;
            }
            mesh.vertices = vertices;
            mesh.uv = texcoords;
            texture.LoadRawTextureData(image);
            texture.Apply();
        }
    }
}

```



ポイントは以下の通り。

* ~~Unityのメッシュは65536インデックスまでしか表現できないらしい。全然足りないので子オブジェクトをつくって分割する。~~
* `Mesh.indexFormat` に **`UnityEngine.Rendering.IndexFormat.UInt32` を指定することでインデックス上限が 32bit になる** らしい.もうメッシュを分ける必要ないらしい.
* ポリゴンでなくポイントで出力するために、`Mesh.SetIndices(*, MeshTopology.Points, *)` を使う ([参考](http://nn-hokuson.hatenablog.com/entry/2017/06/06/202953)) 。[^2]

[^2]: 三角でなくていいから番号は連番で十分だ。


あとは、先程のUnityサンプルシーンに EmptyObject を足してこのスクリプトをアタッチすればOK。
邪魔なオブジェクトは消すか disable にしておく。
RealSenseDevice は使うのでそのままで。


![2018-02-25-2.png](https://qiita-image-store.s3.amazonaws.com/0/140877/fbb532e4-83a3-af25-bfb3-6b983b0978af.png)

できた。
上下がさまさまだが、軸の正負が逆なだけなので、Rotation や Scale で調整すればよいだろう。

<br>
ちょうどいい難易度で、忘れかけてた Unity 開発を思い出すのに手頃なテーマではあった。
しかし、初歩からこんな調子では先が思いやられる・・・。[^3]

[^3]: C# より C++ の方が慣れているが、Unity の方が後々いろいろ遊べるだろうと思って敢えて Unity でやってみたのに。

なお、結局今のところまだ API そのものについては調べられていない。
なので、librealsense についての質問にはおそらく答えられない。

どちらかと言えば、識者に「こうやればもっといいよ」と指摘してもらえることを狙っている。
なんか遠回りしているような気がする。

*2018/02/28 追記*

よく見たら、本家 development ブランチに C# の PointCloud も Unity の PointCloud サンプルも入っていた。
C# ラッパーは似たようなものだったが、Unityサンプルはポイントメッシュでなくパーティクルを使っているようだった。
(動かしてないからどう見えるのかはわからない)

このあとちょっといじってメッシュにしてみようと思っていたので、そういう人には本家のサンプルよりこっちの方がいいかもしれない。

*2019/07/10 追記*
本家のライブラリは本記事を書いた当時からだいぶ変わっていていまさら手直ししたところで、という気もしたが、最近なぜかアクセスが増えてるっぽいので直してみた.
最近また少しいじり始めたが、結構変わってて「また最初からか」という気分.

---