Depth.cpp

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

Copyright (c) 2012 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

-----------------------------------------------------------------------------
*/

#include "generator/Depth.h"
#include "generator/User.h"
#include "DeviceManager.h"

Depth::Depth(KinectDevice* pDevice)
{
  mData = 0;
  mReadData = 0;
  mDepthHist = 0;
  mReadDepthHist = 0;
  mCuttedData = 0;

  iDistCutoff = 10000;
  fAngCutoff = 0.0f;
  mParentDevice = pDevice;
  
  int width = 0;
  int height = 0;
}

Depth::Depth()
{
}

Depth::~Depth()
{
  boost::mutex::scoped_lock l(mmutex);

  if (mData)
    free(mData);

  if (mCuttedData)
    free(mCuttedData);

  if (mReadData)
    free(mReadData);

  if (mDepthHist)
    free(mDepthHist);

  if (mReadDepthHist)
    free(mReadDepthHist);
}

KinectDevice* Depth::GetParentDevice()
{
  return mParentDevice;
}

bool Depth::UpdateData(openni::VideoFrameRef frame)
{
  boost::mutex::scoped_lock l(mmutex);

  if (!frame.isValid())
    return false;

  // init buffers
  if (mData == 0)
  {
    width = frame.getVideoMode().getResolutionX();
    height = frame.getVideoMode().getResolutionY();

    unsigned int dataSize = width * height * sizeof(openni::DepthPixel);
    unsigned int historySize = MAX_DEPTH * sizeof(float);
  
    mData = (unsigned short*)malloc(dataSize);
    memset(mData, 0, dataSize);

    mCuttedData = (unsigned short*)malloc(dataSize);
    memset(mCuttedData, 0, dataSize);

    mReadData = (unsigned short*)malloc(dataSize);
    memset(mReadData, 0, dataSize);

          mDepthHist = (float*)malloc(historySize);
    memset(mDepthHist, 0, historySize);

          mReadDepthHist = (float*)malloc(historySize);
    memset(mReadDepthHist, 0, historySize);
  }

  /*
  if (mDepth.readFrame(&frame) != openni::STATUS_OK)
    return false;
  */

  memcpy(mData, frame.getData(), width * height * sizeof(openni::DepthPixel));
  memcpy(mCuttedData, frame.getData(), width * height * sizeof(openni::DepthPixel));
  DepthCutoff();

  const openni::DepthPixel* pDepth = mData;

  // Calculate the accumulative histogram
  memset(mDepthHist, 0, MAX_DEPTH * sizeof(float));

        unsigned int nNumberOfPoints = 0;
  for (int y = 0; y < frame.getHeight(); ++y)
        {
                for (int x = 0; x < frame.getWidth(); ++x, ++pDepth)
                {
                        if (*pDepth != 0)
                        {
                                mDepthHist[*pDepth]++;
                                nNumberOfPoints++;
                        }
                }
        }
        for (int nIndex = 1; nIndex < MAX_DEPTH; nIndex++)
        {
                mDepthHist[nIndex] += mDepthHist[nIndex-1];
        }
        if (nNumberOfPoints)
        {
                for (int nIndex=1; nIndex < MAX_DEPTH; nIndex++)
                {
                        mDepthHist[nIndex] = 256.0f * (1.0f - (mDepthHist[nIndex] / nNumberOfPoints));
                }
        }
  return true;
}

const unsigned short* Depth::GetBuffer()
{
  return mData;
}

const unsigned short* Depth::GetCuttedBuffer()
{
  return mCuttedData;
}

void Depth::GetBuffer(PtrObjBitmap B)
{
  // Get depth map
  {
    boost::mutex::scoped_lock l(mmutex);   
    if (mData && mDepthHist && mReadData && mReadDepthHist)
    {
      memcpy(mReadData, mData, width * height * sizeof(openni::DepthPixel));
      memcpy(mReadDepthHist, mDepthHist, MAX_DEPTH * sizeof(float));
    }
    else
      return;
  }

  // Test bitmap size
  if ((B->TailleW != width) || (B->TailleH != height))
  {
    return;
  }

  //bool isMirror = KinectOBJ->GetMirrorMode();

  // Copy data to Scol buffer
  int x = 0, y = 0;
  unsigned int destByte = 0;
  int color = 0;
  const openni::DepthPixel* pDepth = mReadData;
  for (y=0; y<height; y++)
  {
    const openni::DepthPixel* pDepthRow = pDepth;
    for (x=0; x<width; x++, ++pDepthRow)
    {
      int color = 0;
      if (*pDepthRow != 0)
        color = (int)mReadDepthHist[*pDepthRow];

      destByte = (x + y * width) * 3;

      B->bits[destByte] = color;
      B->bits[destByte+1] = color;
      B->bits[destByte+2] = color;
    }

    pDepth += width;
  }
}

bool Depth::IsValid()
{
  return mData != 0 ? true : false;
}

void Depth::SetMirror(bool state)
{
  /*
  if (mDepth.isValid())
    mDepth.setMirroringEnabled(state);
*/
}

void Depth::GetDepthSize(int &w, int &h)
{
  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() 
{
  openni::DepthPixel val2Replace = 0;
  int xcut = width;

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

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