インテル® RealSense™ 3Dカメラ を動かしてみる【Colorカメラ画像の表示】

RealSense3Dカメラ(F200)でColorカメラの画像を取得して表示するプログラムを書いてみます。

前回、環境について書きましたので改めて簡単に
Windows 8.1、VisualStudio 2013、WPF C#　での実装となります。

#前提
* テストコードなので、取得する画像のサイズ等数値を決め打ちしているところがある
* テストコードなので、厳密にエラー処理を書いていない
* カメラ制御の処理は別クラスにするなどにしたいが、テストコードなので今回はxaml.csにべた書きする

#処理の流れ
RealSenseSDKでカメラ画像を取得する+WPFで画面表示する処理の流れは、このような感じで行きます。

1. カメラ画像書き込み用のWriteableBitmapを準備する
2. 初期化処理
　　カメラ制御用クラスのオブジェクトを取得、「4.」の処理を実施するTaskを生成
3. Colorカメラストリームを有効にしてカメラのフレーム取得を開始する
4. カメラフレームを取得してフレーム画像をWriteableBitmapに書き込む
5. 終了処理

「4.」の処理を今回はTask内で繰り返して実装します。

#ソース
本記事の全ソースは以下に置きました。
https://github.com/aquaring/RealSenseTestCode
「RealSenseColorCameraWPF」というプロジェクトです。

#実装の説明
##VisualStudioでプロジェクトを作る
・WPFのプロジェクトを作り、Colorカメラの画像を表示するためのImageコントロールを配置する。
　名前は「ColorImage」サイズは「1920 x 1080」にする。
・SDKのC#用ライブラリである「libpxcclr.cs.dll」を参照設定に追加する。
　デフォルトなら C:\Program Files (x86)\Intel\RSSDK\bin\x64\libpxcclr.cs.dll にあります。
・ビルドターゲットを「x64」にします。
・C/C++のライブラリである「libpxccpp2c.dll」を実行ファイルのフォルダに持ってくる
　プロジェクトのプロパティのビルドイベントの「ビルド後に実行するコマンドライン」に
　dllをターゲットに合わせてコピーしてくるコマンドを書きました。

```csharp:ビルド後に実行するコマンドライン
　if "$(Platform)" == "x86" (copy /y "$(RSSDK_DIR)bin\win32\libpxccpp2c.dll" "$(TargetDir)") else (copy /y "$(RSSDK_DIR)bin\x64\libpxccpp2c.dll" "$(TargetDir)")
```
「$(RSSDK_DIR)」は、SDKのディレクトリで、インストール時に環境変数に設定されています。
デフォルトでは「C:\Program Files (x86)\Intel\RSSDK\」です。

##クラスメンバ変数など
```csharp:MainWindows.xaml.cs
/// <summary>
/// クラスメンバ変数
/// </summary>

// RealSenseSDKのセッション
private PXCMSession m_Session = null;

// RealSenseSDKのManager
private PXCMSenseManager m_Cm = null;

// カメラ画像取得を繰り返し処理するためのTask
private Task m_CameraCaptureTask = null;

// タスク続行フラグ
private bool m_TaskContinueFlg = true;

// Colorカメラ画像書き込み用のWriteableBitmap
private WriteableBitmap m_ColorWBitmap;
```
##初期化時の処理
```csharp:MainWindows.xaml.cs
/// <summary>
/// コンストラクタ
/// 
/// カメラ画像表示用のWritableBitmapを準備してImageに割り当て
/// カメラ制御用のオブジェクトを取得して
/// Colorカメラストリームを有効にしてカメラのフレーム取得を開始する
/// カメラキャプチャーするためのTaskを準備して開始
/// </summary>
public MainWindow()
{
    // 画面コンポーネントを初期化
    InitializeComponent();

    // カメラ画像書き込み用のWriteableBitmapを準備してImageコントローラーにセット
    m_ColorWBitmap = new WriteableBitmap(1920, 1080, 96.0, 96.0, PixelFormats.Bgr32, null);
    ColorCameraImage.Source = m_ColorWBitmap;

    // カメラ制御のオブジェクトを取得
    m_Session = PXCMSession.CreateInstance();
    m_Cm = m_Session.CreateSenseManager();

    // Colorカメラストリームを有効にする
    m_Cm.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 1920, 1080, 30);

    // カメラのフレーム取得開始
    pxcmStatus initState = m_Cm.Init();
    if (initState < pxcmStatus.PXCM_STATUS_NO_ERROR)
    {
        // エラー発生
        MessageBox.Show(initState + "\nカメラ初期化に失敗しました。");
        return;
    }

    // カメラ取得画像をミラーモードにする
    m_Cm.captureManager.device.SetMirrorMode(PXCMCapture.Device.MirrorMode.MIRROR_MODE_HORIZONTAL);

    // カメラキャプチャーをするためのタスクを準備して起動
    m_CameraCaptureTask = new Task(() => CaptureCameraProcess());
    m_CameraCaptureTask.Start();
}
```

・カメラ画像書き込み用のWriteableBitmapを準備する
　Colorカメラから取得できる画像のピクセルフォーマットは
　RealSenseでは、PXCMImage.PixelFormat列挙型に定義されていて「PIXEL_FORMAT_RGB32」です。

　これをリファレンスを見ると
　　The 32-bit RGB32 color format. On little endian machines, the memory layout is BGRA.
　　See fourcc.org for the description and memory layout.

　と説明があります。

　これはつまり、1ピクセルを表すのに、B、G、R、Aそれぞれ1バイトずつ順に配置されて4バイトになるということなので
　これと同じメモリ配置になるように「PixelFormats.Bgr32」でWriteableBitmapを作成しています。

・カメラ制御用クラスのオブジェクトを取得する
　PXCMSessionクラスのインスタンス(m_Session)はCreateメソッドで取得します。
　このインスタンスで接続されているカメラデバイスの情報を取得したりします。
　PXCMSessionクラスのインスタンス(m_Session)からPXCMSenseManagerクラスのインスタンス(m_Cm)を取得します。

　今回はカメラは1台の前提なのでどのカメラデバイスを使用するかを指定しませんが、複数のデバイスがある場合は
　m_Sessionで取得したデバイス情報をm_Cmに指定して(m_CmIntel.captureManager.FilterByDeviceInfo(deviceInfoIntel);
　こんな感じ)どのカメラデバイスを使用するかを決めるようです。
　つまり複数のカメラデバイスを同時に扱う場合には、PXCMSenseManagerクラスのインスタンスを
　使用するカメラデバイスの数分生成するイメージになります。

・Colorカメラストリームを有効にしてカメラのフレーム取得を開始する
　やっていることは、コメントの通りですが簡単にエラー処理もしています。
　エラー処理は、Init()等のメソッドを呼び出すとpxcmStatus型で結果が返るのでその値を確認して行う感じになります。
　(Exceptionがthrowされるのではないんですね・・・)

　あと、画面に対峙して見るので、表示された画像をミラーモードにしています。

・カメラフレームの処理を実施するTaskを生成
　画面周りのスレッドとは別に非同期でカメラフレームを取得するために
　今回はTaskで実装します。
　(CaptureCameraProcess1()の処理は後述します)

##カメラフレームを取得してフレーム画像をWriteableBitmapに書き込む
Taskで別スレッドで呼び出して、繰り返し処理します。

```csharp:MainWindows.xaml.cs
/// <summary>
/// カメラキャプチャーの処理
/// 　Taskで実行し、Waitをはさみながら常に別スレッドで実行する
/// </summary>
private void CaptureCameraProcess()
{
    while (m_TaskContinueFlg)
    {
        int wbStride = 0;
	Int32Rect wbRect = new Int32Rect(0, 0, 0, 0);
	Byte[] buffer = null;

	// フレームを取得してサンプリング
	pxcmStatus statusFrame = m_Cm.AcquireFrame(true);
	PXCMCapture.Sample sample = m_Cm.QuerySample();
	PXCMImage.ImageData imageData = null;

	// ColorサンプリングからImageDataを読み込んで取得
	pxcmStatus sts = sample.color.AcquireAccess(
                                           PXCMImage.Access.ACCESS_READ, 
                                           PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, 
                                           out imageData);
	if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
	{
	    // ImageDataが取得できたらWritableBitmapに書き込むためにバイト配列に変換する
	    int length = imageData.pitches[0] * sample.color.info.height;
	    buffer = imageData.ToByteArray(0, length);

	    // ストライドと描画領域を取得
	    wbStride = imageData.pitches[0];
	    wbRect = new Int32Rect(0, 0, sample.color.info.width, sample.color.info.height);

	    // Colorサンプリングのアクセスを終了する
	    sample.color.ReleaseAccess(imageData);

	    // フレームデータをビット配列にしてWriteableBitmapに書き込む
	    m_ColorWBitmap.Dispatcher.BeginInvoke
	    (
	        new Action(() =>
	        {
	            m_ColorWBitmap.WritePixels(wbRect, buffer, wbStride, 0);
	        }
	    ));
	}

        // フレームを解放
        m_Cm.ReleaseFrame();

        // 少しWaitする
        Thread.Sleep(20);
    }

    // -----------
    //  終了処理
    // -----------
    // カメラの終了処理
    m_Cm.Close();
    m_Cm.Dispose();
    m_Session.Dispose();

    // Windowの終了処理　画面処理のスレッドで実施するようにInvokeする
    MainWin.Dispatcher.BeginInvoke
    (
        new Action(() =>
        {
            App.Current.Shutdown();
        }
    ));
}
```
コメントに書いてある通りですが
カメラフレームとサンプリンを取得してImageDataを取得しています。
RealSenseカメラの画像ピクセルデータをWritableBitmapに書き込むためにバイト配列に変換して
WritableBitmapに書き込んでいます。
(書き込みはスレッドが異なるためWritableBitmapのDispatcher.BeginImvokeで行っています)

このような処理を繰り返し行って、カメラ画像を画面に表示していきます。

##終了処理
Windowの「×」ボタンを押した時のClosingイベントの処理で、Task続行フラグを下してTaskを終了させます。
Windowクローズ自体はキャンセルしてます。

```csharp:MainWindows.xaml.cs
/// <summary>
/// Windowを閉じるときに発生するイベント
/// 　Task続行フラグを下してTaskを終了させる
/// 　Windowのクローズ処理はキャンセルする
/// </summary>
private void MainWin_Closing(object sender, System.ComponentModel.CancelEventArgs e)
{
    // Taskを終了させるために続行フラグを下す
    m_TaskContinueFlg = false;

    // Windowクローズ処理をキャンセル
    e.Cancel = true;
}
```

##所感
SDKは、リファレンスもちゃんと書かれていて(英語ですが)直感的に使える感じなので
非常に使いやすいなぁと感じました。

WPFで画像を表示させるためにWritableBitmapを使用していますが
その際に、RealSenseのImageDataをバイト配列にしてWritableBitmapに書き込んでいます。
これがスマートな方法かはわからないのですが、私は実案件での実装にWPFを使うことが多いので
実際につかうときにもWPFであればこの方法で行くのかなぁと思います。

きっとOpenCVと組み合わせて画像処理などをするときは、OpenCvSharpでやるか
C++で書いてしまうか迷うところですが、IFが必要なものであれば恐らくWPFで行くのかなと思います。