Kinect XNAで奥行きの取得 OpenNI V1.1.041版

　前回「Kinect XNAで奥行きの取得」を解説したのですが、OpenNIをV1.1.041版にアップグレードすると仕様が変わったようで動かないので、ここで解説しておきます。

using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using OpenNI;


namespace OpenKinect
{
 /// <summary>
 /// 基底 Game クラスから派生した、ゲームのメイン クラスです。
 /// </summary>
 public class Game1 : Microsoft.Xna.Framework.Game
 {
  GraphicsDeviceManager graphics;
  SpriteBatch spriteBatch;

  private readonly string SAMPLE_XML_FILE = @"C:\Program Files (x86)\OpenNI\Data\SamplesConfig.xml";

  Texture2D texture = null;         //テクスチャ
   private Color[] imageColor;        //色情報を格納する

  #region Kinect関係
  private Context context;
  private DepthGenerator depth;
  private UserGenerator userGenerator;
  private SkeletonCapability skeletonCapbility;
  private PoseDetectionCapability poseDetectionCapability;
  private string calibPose;
  private bool shouldRun;
  private int[] histogram;
  private DepthMetaData depthMD = new DepthMetaData();


  private Dictionary<int, Dictionary<SkeletonJoint, SkeletonJointPosition>> joints;

  private bool shouldDrawPixels = true;
  private bool shouldDrawBackground = true;
  private bool shouldPrintID = true;
  private bool shouldPrintState = true;
  private bool shouldDrawSkeleton = true;

  private Color[] colors = { Color.Red, Color.Blue, Color.ForestGreen, Color.Yellow,
           Color.Orange, Color.Purple, Color.White };
  private int ncolors = 6;

  #endregion

  public Game1()
  {
   graphics = new GraphicsDeviceManager(this);
   Content.RootDirectory = "Content";


   this.TargetElapsedTime = TimeSpan.FromSeconds(1.0 / 30.0);
  }

  protected override void Initialize(){

   base.Initialize();

   #region KINECT
    this.context = new Context(SAMPLE_XML_FILE);
    this.depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
    if (this.depth == null)
    {
     throw new Exception("Viewer must have a depth node!");
    }

    this.userGenerator = new UserGenerator(this.context);
    this.skeletonCapbility = this.userGenerator.SkeletonCapability;
    this.poseDetectionCapability = this.userGenerator.PoseDetectionCapability;
    this.calibPose = this.skeletonCapbility.CalibrationPose;

    this.userGenerator.NewUser += userGenerator_NewUser;
    this.userGenerator.LostUser += userGenerator_LostUser;
    this.poseDetectionCapability.PoseDetected += poseDetectionCapability_PoseDetected;
    this.skeletonCapbility.CalibrationEnd += skeletonCapbility_CalibrationEnd;

    this.skeletonCapbility.SetSkeletonProfile(SkeletonProfile.All);
    this.joints = new Dictionary<int,Dictionary<SkeletonJoint,SkeletonJointPosition>>();
    this.userGenerator.StartGenerating();


    this.histogram = new int[this.depth.DeviceMaxDepth];

    MapOutputMode mapMode = this.depth.MapOutputMode;

   #endregion

  }

  #region Kinect関数
   #region // キャリブレーションが完了しトラッキングするかどうか
   void skeletonCapbility_CalibrationEnd(object sender, CalibrationEndEventArgs e){
    if (e.Success){
     this.skeletonCapbility.StartTracking(e.ID);
     this.joints.Add(e.ID, new Dictionary<SkeletonJoint, SkeletonJointPosition>());
    }else{
     this.poseDetectionCapability.StartPoseDetection(calibPose, e.ID);
    }
   }
   #endregion

   #region // キャリブレーションの認識
   void poseDetectionCapability_PoseDetected(object sender, PoseDetectedEventArgs e){
    this.poseDetectionCapability.StopPoseDetection(e.ID);
    this.skeletonCapbility.RequestCalibration(e.ID, true);
   }
   #endregion

   #region 新しいユーザの検出
   void userGenerator_NewUser(object sender, NewUserEventArgs e){
    this.poseDetectionCapability.StartPoseDetection(this.calibPose, e.ID);
   }
   #endregion

   #region ユーザの消滅(ロスト)
   void userGenerator_LostUser(object sender, UserLostEventArgs e){
    this.joints.Remove(e.ID);
   }
   #endregion

   #region CalcHist
   private unsafe void CalcHist(DepthMetaData depthMD){
    // reset
    for (int i = 0; i < this.histogram.Length; ++i)
     this.histogram[i] = 0;

    ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();

    int points = 0;
    for (int y = 0; y < depthMD.YRes; ++y){
     for (int x = 0; x < depthMD.XRes; ++x, ++pDepth){
      ushort depthVal = *pDepth;
      if (depthVal != 0){
       this.histogram[depthVal]++;
       points++;
      }
     }
    }
    for (int i = 1; i < this.histogram.Length; i++){
     this.histogram[i] += this.histogram[i-1];
    }

    if (points > 0){
     for (int i = 1; i < this.histogram.Length; i++){
      this.histogram[i] = (int)(256 * (1.0f - (this.histogram[i] / (float)points)));
     }
    }
   }
   #endregion

  #endregion

  #region LoadContent
  protected override void LoadContent(){
   spriteBatch = new SpriteBatch(GraphicsDevice);
  }
  #endregion

  #region UnloadContent
  protected override void UnloadContent(){ }
  #endregion

  #region Update
  protected unsafe override void Update(GameTime gameTime){
   if(GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
    this.Exit();

   #region Kinect読み込み

   try{
    this.context.WaitOneUpdateAll(this.depth);// 深度の更新を待つ
   }catch (Exception){
   }

   this.depth.GetMetaData(depthMD);

   CalcHist(depthMD);

   #region　画像の生成
    lock (this) {
 
     ushort* pDepth = (ushort*)this.depth.DepthMapPtr.ToPointer();  //奥行のポインタ取得
     ushort* pLabels = (ushort*)this.userGenerator.GetUserPixels(0).LabelMapPtr.ToPointer();

     this.imageColor = new Color[depthMD.XRes * depthMD.YRes];
     this.texture = new Texture2D(graphics.GraphicsDevice , depthMD.XRes , depthMD.YRes); //テクスチャの作成

     //画像取得　ポインタからの情報をX,Y軸に格納していく
     for (int y = 0; y < depthMD.YRes; ++y){ //y軸
      for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, ++pLabels){

       ushort label = *pLabels;
       if (this.shouldDrawBackground || *pLabels != 0){
        Color labelColor = Color.White;
         if (label != 0)
          labelColor = colors[label % ncolors];
         byte pixel = (byte)this.histogram[*pDepth];
         this.imageColor[ y*depthMD.XRes+x ] = new Color( (byte)(pixel * (labelColor.R / 256.0)),
                      (byte)(pixel * (labelColor.G / 256.0)),
                      (byte)(pixel * (labelColor.B / 256.0)) );
       }
      }
     }
     
     this.texture.SetData(this.imageColor); //textureにデータを書き込む
    }
   #endregion
   #endregion

    base.Update(gameTime);
  }
  #endregion

  #region Draw
  protected override void Draw(GameTime gameTime){
   GraphicsDevice.Clear(Color.CornflowerBlue);


   //スプライト
   this.spriteBatch.Begin();
   this.spriteBatch.Draw(this.texture,new Vector2(0,0), Color.White); //Textureの描写
   this.spriteBatch.End();

   base.Draw(gameTime);
  }
  #endregion

 }
}