Kinect SDK XNA 奥行から、なんちゃって3Dスキャナ　その１

using System.Collections.Generic;
using System.IO;
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 Microsoft.Research.Kinect.Nui;    //Kinect Uniの読み込み

namespace WindowsGame6
{
  public class Game1 : Microsoft.Xna.Framework.Game
  {
    GraphicsDeviceManager graphics;
    SpriteBatch spriteBatch;

    Texture2D texture_image = null; //実画像テクスチャ
    Texture2D texture_depth = null; //奥行テクスチャ

    private  Color[]    imageColor; //色情報の格納
    private  Color[]    depthColor; //色情報の格納
    private  float[]    depth;

    private List<float[]>  List_depth;

    private Boolean isSpace =false;

    private Vector3 CameraT = new Vector3(0.0f,0.0f,3.0f);
    private Vector3 CameraR = new Vector3(0.0f,0.0f,0.0f);
    private Vector3 CameraF = new Vector3(0.0f,0.0f,0.0f);



      private SpriteFont font = null;   //フォント
    #region ポリゴン
      private VertexBuffer  vertexBuffer= null;  //頂点バッファ
      private IndexBuffer    indexBuffer = null; //インデックスバッファ
      private  VertexPositionNormalTexture[] vertices =null;
      private Int16[]      vertexIndices;
    #endregion

    private BasicEffect basicEffect = null;

    Runtime nui;    //Kinectのセンサクラス



    public Game1(){
      graphics = new GraphicsDeviceManager(this);
      Content.RootDirectory = "Content";
      //画面サイズを640 X 480
      this.graphics.PreferredBackBufferWidth = 640; 
      this.graphics.PreferredBackBufferHeight= 480;
    }

    #region Initialize
    protected override void Initialize(){

      base.Initialize();

      this.List_depth = new List<float[]>();


        //フォントの読み込み
         this.font = this.Content.Load<SpriteFont>("Font");



      #region Kinect初期化
      nui = new Runtime();    //Kinectセンサクラスの初期化

      try{
        //奥行の取得、トラッキング、実画像
        nui.Initialize(    RuntimeOptions.UseDepthAndPlayerIndex |
        RuntimeOptions.UseSkeletalTracking |
        RuntimeOptions.UseColor);
      }
      catch (InvalidOperationException){
        Console.WriteLine("Runtime initialization failed.");
      return;
      }
      try{
        //ビデオストリームを開く
        nui.VideoStream.Open(    ImageStreamType.Video, 
                    2, 
                    ImageResolution.Resolution640x480,
                    ImageType.Color);
      //デプスストリームを開く
      nui.DepthStream.Open(    ImageStreamType.Depth,
                    2, 
                    ImageResolution.Resolution320x240, 
                    ImageType.DepthAndPlayerIndex);

      }catch (InvalidOperationException){
        Console.WriteLine("Failed to open stream. ");
        return;
      }

      //フレーム更新毎にnui_DepthFrameReadyを呼び出す
      nui.DepthFrameReady += 
      new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);
            
      //フレーム更新毎にnui_ColorFrameReadyを呼び出す
      nui.VideoFrameReady += 
      new EventHandler<ImageFrameReadyEventArgs>(nui_ColorFrameReady);

      #endregion

    }
    #endregion

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

      this.imageColor = new Color[640 * 480];
      this.depthColor = new Color[320* 240];
      this.depth = new float[ 320 *240];

      #region basicEffect
        // エフェクトを作成
        this.basicEffect = new BasicEffect(this.GraphicsDevice);

        //頂点バッファ作成 OK
        this.vertexBuffer = new VertexBuffer(
          this.GraphicsDevice,typeof(
              VertexPositionNormalTexture),
              (160*120),
              BufferUsage.None);
        //頂点データを作成する OK 
        this.vertices =  new VertexPositionNormalTexture[ (160*120)];

        // インデックスバッファを作成
        this.indexBuffer = new IndexBuffer(
              this.GraphicsDevice,
              IndexElementSize.SixteenBits,
              (160*120)*6, BufferUsage.None);

        this.vertexIndices = new Int16[ 160*120 * 6];

        // テクスチャーの使用を許可する
        this.basicEffect.TextureEnabled = true;

        // プロジェクションマトリックスをあらかじめ設定
        this.basicEffect.Projection = Matrix.CreatePerspectiveFieldOfView(
          MathHelper.ToRadians(45.0f),
          (float)this.GraphicsDevice.Viewport.Width /
          (float)this.GraphicsDevice.Viewport.Height,
          1.0f,
          10000.0f
        );
      #endregion

    }
    #endregion

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

    #region Update
    protected override void Update(GameTime gameTime)
    {

      #region ゲームパッド
        Vector2 LV = new Vector2();  
        Vector2 RV = new Vector2();  

        //ゲームパッド
        LV.X = GamePad.GetState(Microsoft.Xna.Framework.PlayerIndex.One).ThumbSticks.Left.X;
        LV.Y = GamePad.GetState(Microsoft.Xna.Framework.PlayerIndex.One).ThumbSticks.Left.Y;
        RV.X = GamePad.GetState(Microsoft.Xna.Framework.PlayerIndex.One).ThumbSticks.Right.X;
        RV.Y = GamePad.GetState(Microsoft.Xna.Framework.PlayerIndex.One).ThumbSticks.Right.Y;

    
        #region 回転
          if(RV.Y >  0.5){   this.CameraR.X -= 1.0f;   }
          if(RV.Y < -0.5){   this.CameraR.X += 1.0f;   }
          if(RV.X >  0.5){   this.CameraR.Z += 1.0f;   }
          if(RV.X < -0.5){   this.CameraR.Z -= 1.0f;   }

          #endregion

        #region カメラ移動
          #region 前後移動
            Vector3 move = Vector3.TransformNormal( new Vector3(0,0,0.1f),
                Matrix.CreateRotationY( MathHelper.ToRadians(-this.CameraR.Z) ));
              //前へ
              if(LV.Y >0.5){
                this.CameraT -=  move;
                this.CameraF -=  move;
              }

              //後ろへ
              if(LV.Y < -0.5){
                this.CameraT +=  move;
                this.CameraF +=  move;
              }
          #endregion

          #region 左右移動
            move = Vector3.TransformNormal(  new Vector3(0.1f,0.0f,0.0f),
                Matrix.CreateRotationY( MathHelper.ToRadians(-this.CameraR.Z) ));
              //右へ
              if( LV.X >0.5 ){
                this.CameraT +=  move;
                this.CameraF +=  move;
              }
              //左へ
              if( LV.X <-0.5){
                this.CameraT -=  move;
                this.CameraF -=  move;
              }

          #endregion

          

        #endregion
      #endregion

      #region キーボード処理
      KeyboardState keyState = Keyboard.GetState();
      keyState = Keyboard.GetState();

      
      if(keyState.IsKeyDown(Keys.S)){
        SaveJpeg();  //テクスチャの保存
        SaveOBJ();  //OBJファイルの保存
          
      }
      


      if(keyState.IsKeyDown(Keys.Space)){
        this.isSpace = true;
      }else{
        this.isSpace = false;
      }
      #endregion



      #region カメラ座標計算
      Matrix  m_rz = Matrix.CreateRotationY( MathHelper.ToRadians( this.CameraR.Z));
      Matrix  m_rx = Matrix.CreateRotationX( MathHelper.ToRadians( this.CameraR.X));
      Matrix  m_trans = Matrix.CreateTranslation(this.CameraT);

      Matrix  m_look = Matrix.CreateLookAt( this.CameraT , this.CameraF , new Vector3(0,1,0));
      Matrix  tmpR = Matrix.Identity;
          tmpR =Matrix.Multiply( tmpR  , m_rz);
          tmpR =Matrix.Multiply( tmpR  , m_rx);

          tmpR =Matrix.Multiply( m_look, tmpR);
//          tmpR =Matrix.Multiply( tmpR,  m_look);
      #endregion

      #region 3D処理
          #region 頂点処理
          float Psize =0.02f;
          for(int y=0;y<120;y++){
            for(int x=0; x<160;x++){

              //UV座標設定
              float ux =0.25f+ (x)/160.0f*0.5f;  
              float uy =0.25f+ (y)/120.0f*0.5f;
              
              //Z座標を設定
              float d= this.depth[y*160+x];
              float z= 0;
              
              if(d == 0){
                z = -20;
            //  Console.WriteLine( d);
              }else{
                z =(((-d/3975.0f)*20.0f));
              }
              //ポリゴン生成
              vertices[ y*160 + x ] = new VertexPositionNormalTexture(
              new Vector3((x)*Psize -1.6f, 0-(y)*Psize +1.2f, z),
              new Vector3(0,0,1), new Vector2(ux,uy));
            }
          }

          //頂点データを頂点バッファに書き込む
          this.vertexBuffer.SetData(vertices);
          #endregion

          #region インデックス処理
          int na=0;
          for(int y=0;y<120-1;y++){
            for(int x=0; x<160-1;x++){
              vertexIndices[na+0] = (short)((y+0)*160+ x+0);
              vertexIndices[na+1] = (short)((y+0)*160+ x+1);
              vertexIndices[na+2] = (short)((y+1)*160+ x+0);

              vertexIndices[na+3] = (short)((y+0)*160+x+1);
              vertexIndices[na+4] = (short)((y+1)*160+x+1);
              vertexIndices[na+5] = (short)((y+1)*160+x+0);          
              na+=6;
            }

          }
          // 頂点インデックスを書き込む
          this.indexBuffer.SetData(vertexIndices);
        #endregion
      #endregion

      // ビューマトリックスを設定
      this.basicEffect.View =  tmpR ;
      base.Update(gameTime);
    }
    #endregion

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

      if(this.texture_image!=null){
        this.basicEffect.Texture = this.texture_image;

        #region オブジェクト描写
        RasterizerState rs = new RasterizerState();
        rs.CullMode = CullMode.None;
    
        if(this.isSpace==true){
          rs.FillMode = FillMode.WireFrame;
        }
        this.GraphicsDevice.RasterizerState = rs;

        DepthStencilState ds = new DepthStencilState();
        ds.DepthBufferEnable =true;
        GraphicsDevice.DepthStencilState = ds;

      // エフェクトでライトを有効にする
            this.basicEffect.LightingEnabled = true;

            // デフォルトのライトの設定を使用する
            this.basicEffect.EnableDefaultLighting();

            // スペキュラーを無効
            this.basicEffect.SpecularColor = Vector3.Zero;

            // ２番目と３番目のライトを無効
            this.basicEffect.DirectionalLight1.Enabled = false;
            this.basicEffect.DirectionalLight2.Enabled = false;


        // 画面を指定した色でクリアします
        this.GraphicsDevice.Clear(Color.CornflowerBlue);

        // 頂点バッファをセットします
        this.GraphicsDevice.SetVertexBuffer(this.vertexBuffer);

        // インデックスバッファをセット
        this.GraphicsDevice.Indices = this.indexBuffer;

        // パスの数だけ繰り替えし描画
        foreach (EffectPass pass in this.basicEffect.CurrentTechnique.Passes)
        {
          // パスの開始
          pass.Apply();

          // ポリゴン描画する
          // インデックスを使用してポリゴンを描画する
          this.GraphicsDevice.DrawIndexedPrimitives(
            PrimitiveType.TriangleList,
            0,
            0,
            (160*120),
            0,
            160*120*2
          );
        }
        #endregion
      }

      #region テクスチャ
      this.spriteBatch.Begin();

      //実画像Textureの描写
      if(this.texture_image !=null){
        this.spriteBatch.Draw(this.texture_image,new Vector2(0,0), null, 
        Color.White,0.0f,Vector2.Zero,0.25f,SpriteEffects.None,0.0f);
      }

      //奥行画像Textureの描写
      if(this.texture_depth !=null){
        this.spriteBatch.Draw(this.texture_depth,new Vector2(160,0), null, 
        Color.White,0.0f,Vector2.Zero,0.5f,SpriteEffects.None,0.0f); 
      }

      this.spriteBatch.End();
      #endregion

      base.Draw(gameTime);
    }
    #endregion

    #region 実画像処理
    void nui_ColorFrameReady(object sender, ImageFrameReadyEventArgs e)
    {
      // 32-bit per pixel, RGBA image
      lock(this){
        PlanarImage Image = e.ImageFrame.Image;
        this.texture_image = new Texture2D(graphics.GraphicsDevice,640,480);/テクスチャの作成

        int no=0;

        //画像取得
        for (int y = 0; y < Image.Height; ++y){ //y軸
          for (int x = 0; x < Image.Width; ++x, no += 4){ //x軸
            this.imageColor[ y*  Image.Width +x ] =
              new Color(  Image.Bits[no+2],Image.Bits[no+1],Image.Bits[no+0] );

          }
        }

        this.texture_image.SetData(this.imageColor);    //texture_imageにデータを書き込む
      }

    }
    #endregion

    #region　奥行き画像
    void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e){

      lock(this){
        PlanarImage Image = e.ImageFrame.Image;

        //奥行き画像初期化
        this.texture_depth = new Texture2D(graphics.GraphicsDevice ,
          Image.Width, Image.Height);    //テクスチャの作成
                
        #region 画像生成
        //画像取得
        int ns=0;
        for (int y = 0; y < Image.Height; ++y){ //y軸
          for (int x = 0; x < Image.Width; ++x){ //x軸
            int n= (y* Image.Width +x)*2;
            int realDepth = (Image.Bits[n +1 ] <<5 )|(Image.Bits[ n ] >>3 );

            int player = Image.Bits[ (y*320 +x)*2 ]& 0x07;

            if( player ==0 ){
              realDepth =0;
            }

            //真ん中 160x120の領域切り出し
            if( y >=60 && y< 180 && x >=80 && x< 240 ){
              this.depth[ ns] = realDepth;  //Depth格納
              ns++;

            }
            byte intensity = (byte)((255 - (255 * realDepth / 0x0fff))/2);
            this.depthColor[ y*320 +x   ] =    new Color(intensity , intensity ,intensity);

              #region 領域指定
              if(x == 80  || x == 240 ){
                this.depthColor[ y*  Image.Width +x ] = Color.Green; //縦線の描写
              }else if(y == 60  ||  y == 180){
                this.depthColor[ y*  Image.Width +x ] = Color.Red;   //横線の描写
              }
              #endregion

          }
        }
        //texture_imageにデータを書き込む
        this.texture_depth.SetData(this.depthColor); 
        #endregion

      
        
  
      }
    }
    #endregion

    #region 画像保存
    void SaveJpeg(){
      try{
       Stream stream = File.OpenWrite("capture.jpg");
       texture_image.SaveAsJpeg(stream,640,480);
      }cath{}
    }
    #endregion

    #region OBJ保存
    void SaveOBJ(){
      //例外処理はしていないので注意

      //書き込むファイルが既に存在している場合は、上書きする
      System.IO.StreamWriter sw = new System.IO.StreamWriter(
      @"capture.obj",
        false,
      System.Text.Encoding.GetEncoding("shift_jis"));
      
      sw.WriteLine("#Kinect Capture");
      sw.WriteLine("g obj");

      #region v出力 ベクトル
      for(int y=0;y<120;y++){
        for(int x=0; x<160;x++){
          //ポリゴン生成
          sw.WriteLine("v\t" + vertices[ y*160 + x ].Position.X +
                          "\t" + vertices[ y*160 + x ].Position.Y +
                          "\t" +vertices[ y*160 + x ].Position.Z);
        }
      }
      #endregion
      sw.WriteLine("\n");

      #region vt出力 テクスチャUV
      for(int y=0;y<120;y++){
        for(int x=0; x<160;x++){
          //UV座標設定
          float ux =0.25f+ (x)/160.0f*0.5f;  
          float uy =0.25f+ (y)/120.0f*0.5f;
          sw.WriteLine("vt\t" + ux +"\t" + -uy);
        }
      }
      #endregion
      sw.WriteLine("\n");


      #region vn出力 法線ベクトル
      for(int y=0;y<120;y++){
        for(int x=0; x<160;x++){
          //ポリゴン生成
          sw.WriteLine("vn\t0\t0\t-1");
        }
      }
      #endregion
      sw.WriteLine("\n");

      #region インデックス処理
      for(int y=0;y<120-1;y++){
        for(int x=0; x<160-1;x++){
          int p0  = (short)((y+0)*160+ x+0)+1;
          int p1  = (short)((y+0)*160+ x+1)+1;
          int p2  = (short)((y+1)*160+ x+1)+1;
          int p3  = (short)((y+1)*160+ x+0)+1;
          sw.WriteLine("f\t"  + p0 +"/"+p0 +"/"+p0 +"/"+ "\t" 
                    + p1 +"/"+p1 +"/"+p1 +"/"+ "\t"
                    + p2 +"/"+p2 +"/"+p2 +"/"+ "\t"
                    + p3 +"/"+p3 +"/"+p3 );
        }
      }
      // 頂点インデックスを書き込む
      this.indexBuffer.SetData(vertexIndices);
      #endregion
      sw.Close();
    }
    #endregion

  }
}