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

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

librealsense.x64のインストール

「ツール」→「NuGetパッケージマネージャー」→「ソリューションのNuGetパッケージの管理」→「参照」の中から「librealsense.x64」見つけてインストールする．

初期設定

「Any CPU」を「x64」に変更する．

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

RealSenseクラスの作成

ファイルの冒頭に「using Intel.RealSense;」と記入した上で，以下のRealSenseクラスを作成する．

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内での実装例．

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で実行した場合，シミュレータ上に以下のような点群データが出現する．

ここでは141×80個の点しか表示していないが，変数WidthPointNum（既定値141），HeightPointNum（既定値80）を大きくすると，より多くの点が表示される．

追記

IMUを搭載したモデルであれば，以下のコードでカメラの姿勢をクォータニオンで取得できます．

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;
}