読者です 読者をやめる 読者になる 読者になる

Natural Software

KinectなどのDepthセンサーを中心に活動しています

OpenNI で距離データを扱う( C# + WPF ) #openni_ac

Kinect

このエントリはOpenNI Advent Calendar 2011 : ATNDの12月5日分です!!
Advent Calendarでの、僕の全プロジェクトはこちらです


前回、RGBカメラのデータを扱ったので、今回はKINECTの特長の一つである距離カメラを使ってみます。
距離データからこんな画像を表示してみましょう。

見た目の解説

前回同様、Imageを一つはっつけてます

<Window x:Class="DepthWPF.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        Title="MainWindow" Height="521" Width="661" Closing="Window_Closing">
    <Grid>
        <Image Height="480" HorizontalAlignment="Left" Name="image1" Stretch="Fill" VerticalAlignment="Top" Width="640
               " />
    </Grid>
</Window>

中身の解説

using System;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Threading;
using OpenNI;
using System.Windows.Threading;

namespace DepthWPF
{
    /// <summary>
    /// MainWindow.xaml の相互作用ロジック
    /// </summary>
    public partial class MainWindow : Window
    {
        Context context;
        DepthGenerator depth;

        private Thread readerThread;
        private bool shouldRun;

        public MainWindow()
        {
            InitializeComponent();

            try {
                // ContextとImageGeneratorの作成
                ScriptNode node;
                context = Context.CreateFromXmlFile( "SamplesConfig.xml", out node );
                context.GlobalMirror = false;
                depth = context.FindExistingNode( NodeType.Depth ) as DepthGenerator;

                // 画像更新のためのスレッドを作成
                shouldRun = true;
                readerThread = new Thread( new ThreadStart( () =>
                {
                    while ( shouldRun ) {
                        context.WaitAndUpdateAll();
                        DepthMetaData depthMD = depth.GetMetaData();

                        // ImageMetaDataをBitmapSourceに変換する(unsafeにしなくてもOK!!)
                        this.Dispatcher.BeginInvoke( DispatcherPriority.Background, new Action( () =>
                        {
                            Int16[] depthArray = new Int16[depthMD.XRes * depthMD.YRes];
                            Marshal.Copy( depthMD.DepthMapPtr, depthArray, 0, depthArray.Length );
                            for ( int i = 0; i < depthArray.Length; i++ ) {
                                depthArray[i] = (Int16)(0xffff - (0xffff * depthArray[i] / depth.DeviceMaxDepth));
                            }

                            image1.Source = BitmapSource.Create( depthMD.XRes, depthMD.YRes,
                                96, 96, PixelFormats.Gray16, null, depthArray,
                                depthMD.XRes * depthMD.BytesPerPixel );
                        } ) );
                    }
                } ) );
                readerThread.Start();
            }
            catch ( Exception ex ) {
                MessageBox.Show( ex.Message );
            }
        }

        private void Window_Closing( object sender, System.ComponentModel.CancelEventArgs e )
        {
            shouldRun = false;
        }
    }
}
初期化

今回はDepthのみ使用するので、DepthGeneratorを使います

ScriptNode node;
context = Context.CreateFromXmlFile( "SamplesConfig.xml", out node );
context.GlobalMirror = false;
depth = context.FindExistingNode( NodeType.Depth ) as DepthGenerator;
Depth画像の表示

Imageのときと同じように、Depthを表示します。DepthはGrayscaleで表示するのでひと手間かけてます。
OpenNIのC#メソッドはデータをアンマネージドのポインタで返すので、それをマネージドの配列に変換します。

Int16[] depthArray = new Int16[depthMD.XRes * depthMD.YRes];
Marshal.Copy( depthMD.DepthMapPtr, depthArray, 0, depthArray.Length );


距離データをGrayscaleに変換します。
変換はこちらを参考にしました。16bitの最大値なので、0-0xffff の間に設定し、距離の最大値もGepthGenerator.DeviceMaxDepthから取得しています。

for  ( int i = 0; i < depthArray.Length; i++ ) {
    depthArray[i] = (Int16)(0xffff - (0xffff * depthArray[i] / depth.DeviceMaxDepth));
}

変換したデータを、BitmapSourceとしてImageに設定します。

image1.Source = BitmapSource.Create( depthMD.XRes, depthMD.YRes,
    96, 96, PixelFormats.Gray16, null, depthArray,
    depthMD.XRes * depthMD.BytesPerPixel );
まとめ

OpenNIで距離データを扱う方法でした。
距離データそのものを扱う場合は、

ushort[] depthArray = new ushort[depthMD.DataSize];

depthArrayの各要素が距離になります。