Image.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/Image.h"
#include "DeviceManager.h"

Image::Image(KinectDevice* pDevice)
{
  int width = 0;
  int height = 0;
  bStarted = false;
  mParentDevice = pDevice;

  if (mColor.create(pDevice->GetDevice(), openni::SENSOR_COLOR) == openni::STATUS_OK)
  {
    pDevice->GetGeneratorsSize(width, height);
    if (!SetOutputMode(width, height, 30))
      MMechostr(MSKDEBUG, "OpenNI device : could not set output mode to %dx%d\n", width, height);
  }
  else
  {
    MMechostr(MSKDEBUG, "OpenNI device : could not create image generator\n");
  }
}

Image::Image()
{
}

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

  if (mColor.isValid())
  {
    if (bStarted)
      mColor.stop();
    mColor.destroy();
  }
}

bool Image::IsStarted()
{
  return bStarted;
}

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

bool Image::SetOutputMode(int xRes, int yRes, int nFps)
{
  openni::VideoMode vm;
  vm.setFps(nFps);
  vm.setResolution(xRes, yRes);
  vm.setPixelFormat(openni::PIXEL_FORMAT_RGB888);

  width = xRes;
  height = yRes;

  // stop the stream
  if (bStarted)
  {
    mColor.stop();
    bStarted = false;
  }

  if (mColor.setVideoMode(vm) != openni::STATUS_OK)
    return false;
  
  mData = cv::Mat(height, width, CV_8UC3);

  // start the stream
  if (mColor.start() == openni::STATUS_OK)
    bStarted = true;

  return true;
}

bool Image::UpdateData()
{
  openni::VideoStream* streams[1] = {&mColor};

  //try to start it now
  if (!bStarted)
  {
    if (mColor.start() == openni::STATUS_OK)
      bStarted = true;
    else
      return false;
  }

  int idx;
  if (mParentDevice->GetDevice().isValid() && openni::OpenNI::waitForAnyStream(streams, 1, &idx, 400) != openni::STATUS_OK)
    return false;

  boost::mutex::scoped_lock l(mmutex);
  if (bStarted && mColor.isValid())
  {
    //get the next frame
    openni::VideoFrameRef frame;
    
    if (mColor.readFrame(&frame) != openni::STATUS_OK)
      return false;

    memcpy(mData.data, frame.getData(), mData.total() * sizeof(openni::RGB888Pixel));
    cv::cvtColor(mData, mData, CV_RGB2BGR);
    return true;
  }
  return false;
}

const unsigned char* Image::GetBuffer()
{
  return mData.data;
}

void Image::SetMirror(bool state)
{
  //if (mColor.isValid())
    //mColor.setMirroringEnabled(state);
}

void Image::GetBuffer(PtrObjBitmap B)
{
  cv::Mat buffer;
  {
    boost::mutex::scoped_lock l(mmutex);

    if (!mData.empty())
      mData.copyTo(buffer);
    else
      return;
  }

  if ((B->TailleW != width) || (B->TailleH != height))
    return;

  //Scol bitmap are 32bits aligned
  int sbpl = B->TailleW * 3;
  int dbpl = B->BPL;

  if (sbpl == dbpl)
    memcpy(B->bits, buffer.data, B->TailleW * B->TailleH * B->BytesPP);
  else
    for (long y=0; y<B->TailleH; y++)
    {
      for (long x=0; x<B->TailleW; x++)
      {
        unsigned long srcByte = (x * 3) + (sbpl * y);
        unsigned long destByte = (x * B->BytesPP) + (dbpl * y);

        B->bits[destByte] = buffer.data[srcByte];
        B->bits[destByte + 1] = buffer.data[srcByte + 1];
        B->bits[destByte + 2] = buffer.data[srcByte + 2];
      }
    }
}

void Image::GetGreyBuffer(PtrObjBitmap B)
{
  cv::Mat buffer;
  {
    boost::mutex::scoped_lock l(mmutex);

    if (!mData.empty())
      mData.copyTo(buffer);
    else
      return;
  }

  if ((B->TailleW != width) || (B->TailleH != height))
    return;

  //Scol bitmap are 32bits aligned
  int sbpl = B->TailleW * 3;
  int dbpl = B->BPL;

  // Browse all pixels
  for (long y=0; y<B->TailleH; y++)
  {
    for (long x=0; x<B->TailleW; x++)
    {
      unsigned long srcByte = (x * 3) + (sbpl * y);
      unsigned long destByte = (x * B->BytesPP) + (dbpl * y);
      int avcolor = (buffer.data[srcByte] + buffer.data[srcByte + 1] + buffer.data[srcByte + 2]) / 3;
      B->bits[destByte] = avcolor;
      B->bits[destByte + 1] = avcolor;
      B->bits[destByte + 2] = avcolor;
    }
  }
}