G:/work/subversion/scol-technologies/trunk/plugins/OpenNiSCOL/OpenNiSCOL/generator/Depth.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

#include "Depth.h"
#include "User.h"
#include "../core/Exception.h"
#include "../DeviceManager.h"

namespace NIDevice
{
  namespace Generator
  {
    Depth::Depth(NIDevice::Objects::KinectDevice* pDevice) : NIDevice::Core::GenericGenerator<xn::DepthGenerator, XN_NODE_TYPE_DEPTH>(pDevice)
    {
      iDistCutoff = 10000;
      fAngCutoff = 0.0f;
      SetOutputMode(XN_VGA_X_RES, XN_VGA_Y_RES, 30);
      depthData = (unsigned short*)malloc(XN_VGA_X_RES * XN_VGA_Y_RES * sizeof(XnDepthPixel));
      memset(depthData, 0, XN_VGA_X_RES * XN_VGA_Y_RES * sizeof(XnDepthPixel));
    }

    Depth::Depth() : NIDevice::Core::GenericGenerator<xn::DepthGenerator, XN_NODE_TYPE_DEPTH>(0)
    {
    }

    Depth::~Depth()
    {
      if (depthData)
        free(depthData);
    }

    int Depth::SetOutputMode(int xRes, int yRes, int nFps)
    {
      XnMapOutputMode map;
      map.nXRes = xRes;
      map.nYRes = yRes;
      map.nFPS = nFps;
      nRetVal = GetGenerator().SetMapOutputMode(map);
      if (nRetVal != XN_STATUS_OK)
      {
        // Trigger exception
        KINECT_EXCEPT(NIDevice::Core::ExceptionKinect, GetNiDeviceMessage(), GetObjectTypeAsString());
        return nRetVal;
      }

      return XN_STATUS_OK;
    }

    bool Depth::UpdateData()
    {
      if (GetGenerator().IsValid())
            {
        GetGenerator().GetMetaData(g_depthMD);
        if (g_depthMD.IsDataNew())
        {
          //users mask
          /*
          xn::SceneMetaData udata;
          GetParentDevice()->GetUserGenerator()->GetGenerator().GetUserPixels(-1, udata);
          const unsigned short* ubuf = udata.Data();
          XnDepthPixel* depth = g_depthMD.WritableData(); 

          //mask background pixels
          for (int i=0; i < (udata.XRes() * udata.YRes()); i++)
          {
            if ((ubuf[i] * depth[i]) == 0)
              depth[i] = 0;
          }*/
          DepthCutoff();
          memcpy(depthData, g_depthMD.Data(), g_depthMD.XRes() * g_depthMD.YRes() * sizeof(XnDepthPixel));
          return true;
        }
      }
      return false;
    }

    const unsigned short* Depth::GetDepthBuffer(bool useMutex)
    {
      if (useMutex)
        boost::mutex::scoped_lock lock(GetParentDevice()->m_mutex);

      if (UpdateData())
      {
        xnOSMemSet(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
        int nNumberOfPoints = 0;

        XnDepthPixel nValue;

        xn::DepthGenerator pDepthGen = GetGenerator();

        XnUInt64 nTimeStamp = pDepthGen.GetTimestamp();
        XnUInt32 nDataSize = pDepthGen.GetDataSize();
        const XnDepthPixel* pDepth = pDepthGen.GetDepthMap();
        const XnDepthPixel* pDepthEnd = pDepth + (nDataSize / sizeof(XnDepthPixel));

        while (pDepth != pDepthEnd)
        {
                nValue = *pDepth;

                XN_ASSERT(nValue <= MAX_DEPTH);

                if (nValue != 0)
                {
                        g_pDepthHist[nValue]++;
                        nNumberOfPoints++;
                }

                pDepth++;
        }

        XnUInt32 nIndex;
        for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
        {
                g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
        }
        for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
        {
                if (g_pDepthHist[nIndex] != 0)
                {
                        g_pDepthHist[nIndex] = (nNumberOfPoints-g_pDepthHist[nIndex]) / nNumberOfPoints;
                }
        }
      }
      return depthData;
    }

    float* Depth::GetHistory()
    {
      return g_pDepthHist;
    }

    void Depth::GetDepthSize(int &w, int &h)
    {
      width = g_depthMD.XRes();
      height = g_depthMD.YRes();
      w = width;
      h = height;
    }

    void Depth::SetDistCutoff(int dist)
    {
      iDistCutoff = dist;
    }

    int Depth::GetDistCutoff()
    {
      return iDistCutoff;
    }

    void Depth::SetAngCutoff(float ang)
    {
      fAngCutoff = ang;
    }

    float Depth::GetAngCutoff()
    {
      return fAngCutoff;
    }

    void Depth::DepthCutoff() 
    {
      XnDepthPixel val2Replace = 0;
      XnDepthPixel* depth = g_depthMD.WritableData(); 
      int xcut = g_depthMD.XRes();

      //Kinect IR camera fov 57.8
      if ((fAngCutoff > 0.0f) && (fAngCutoff < 57.7f))
       xcut = (g_depthMD.XRes() / 57.8) * fAngCutoff;
        
      int minxcut = (g_depthMD.XRes() / 2) - (xcut / 2);
      int maxxcut = (g_depthMD.XRes() / 2) + (xcut / 2);
      for(int y(0); y < g_depthMD.YRes(); ++y) 
      {
        for(int x(0); x < g_depthMD.XRes(); ++x) 
        {
          int pos = x+y*g_depthMD.XRes();

          if((x < minxcut) || (x > maxxcut) || (iDistCutoff < depth[pos]))
            depth[pos] = val2Replace;
        }
      }
    } 
  }
}