C#でRealSenseの点群を取得したときの備忘録

C#でRealSenseから点群データを取得したときの備忘録です．点群の描画にはOpenRCFを使っているので，Unityユーザーの方などはDraw関連のコードを適宜読み替えてください．

# librealsense.x64のインストール
「ツール」→「NuGetパッケージマネージャー」→「ソリューションのNuGetパッケージの管理」→「参照」の中から「librealsense.x64」見つけてインストールする．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/310597/fd6f0473-3654-f801-d410-54d74bcb3313.png)

# 初期設定
「Any CPU」を「x64」に変更する．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/310597/da24313d-d34d-d355-870e-b74e360dc9a7.png)

binフォルダと同じ階層に「realsense2.dll」が自動で生成されているので，それを「bin / x64」フォルダ内の「Debug」または「Release」フォルダ内にコピーする．


# RealSenseクラスの作成
ファイルの冒頭に「using Intel.RealSense;」と記入した上で，以下のRealSenseクラスを作成する．
```C#
class RealSense
{
    Pipeline PipeLine = new Pipeline();
    DepthFrame Depth;
    RoughRectangle Polygon = new RoughRectangle(0.01f, 0.01f);
        
    public int WidthPointNum = 141;
    public int HeightPointNum = 80;
        
    public RealSense()
    {
        PipeLine.Start();           
        Polygon.Rotate.SetRx(0.5f * (float)Math.PI);
    }
     
    public void  Draw()
    {
        if (Depth != null)
        {
            for (int i = 0; i < Depth.Width; i += Depth.Width / WidthPointNum)
            {
                for (int j = 0; j < Depth.Height; j += Depth.Height / HeightPointNum)
                {
                    float d = Depth.GetDistance(i, j);
                        
                    if (0.02f < d && d < 2.5f)
                    {
                        Polygon.Position[0] = 0.0018f * (0.1f + d) * (i - Depth.Width / 2);
                        Polygon.Position[1] = d;
                        Polygon.Position[2] = -0.0018f * (0.1f + d) * (j - Depth.Height / 2);
                        Polygon.Color.SetRGB((byte)(100 * d), 0, (byte)(255 - 100 * d));   
                        Polygon.Draw();
                    }
                }
            }                               
        }
    }

    public void GetDepthData()
    {
        FrameSet frames = PipeLine.WaitForFrames(100);
      
        if (Depth == null) Depth = frames.DepthFrame;
        else Depth.CopyFrom(frames.DepthFrame.Data);       
    }
}
```
 変数Polygonは点を表す四角形であり，OpenRCFの機能を用いて生成している．Unityユーザーの方などは，適当な四角系で代用されたい．

 # RealSenseクラスの使用例
OpenRCFのMainWindow.xaml.cs内での実装例．

 ```C#
 class RealSense
{
    // 前述したので略
}

public partial class MainWindow : Window
{
    RealSense RealSense = new RealSense();

    void Draw()
    {
        RealSense.Draw();
    }      

    void Button1_Click(object sender, RoutedEventArgs e)
    {
        RealSense.GetDepthData();
    }
        
    void Button2_Click(object sender, RoutedEventArgs e)
    {
        Parallel.RunEndless(RealSense.GetDepthData, 100);               
    }      
}
 ```
 ボタン1を押すと現在のデプスデータを取得する． ボタン2を押すと関数RealSense.GetDepthData()が100msに1回の周期で並列処理（自動で実行）される．上記のコードをOpenRCFで実行した場合，シミュレータ上に以下のような点群データが出現する．
 ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/310597/0729bde1-33d2-f585-9886-c811b3a18265.png)
ここでは141×80個の点しか表示していないが，変数WidthPointNum（既定値141），HeightPointNum（既定値80）を大きくすると，より多くの点が表示される．

# 追記
IMUを搭載したモデルであれば，以下のコードでカメラの姿勢をクォータニオンで取得できます．
```C#
PoseFrame poseFrame = frames.PoseFrame;

if (poseFrame != null)
{
    float qx = poseFrame.PoseData.rotation.x;
    float qy = poseFrame.PoseData.rotation.y;
    float qz = poseFrame.PoseData.rotation.z;
    float qw = poseFrame.PoseData.rotation.w;
}
```