KinectDevice.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 "objects/KinectDevice.h"
#include "generator/Depth.h"
#include "generator/Image.h"
#include "generator/User.h"
#include "DeviceManager.h"
#include "openNiScolPlugin.h"
#include <scolplugin.h>

// mutex for multi kinect devices
boost::mutex enumerateMutex;

KinectDevice::KinectDevice(unsigned int id) : Thread()
{
  mId = id;
  bConnected = false;
  bMirror = false;
  iDistCutoff = 10000;
  fAngCutoff = 20.0f; //Kinect IR camera fov 57.8
  mDepthGenerator = 0;
  mImageGenerator = 0;
  mUserGenerator = 0;
  msklSmoothing = 0.6f;
  
  // frame buffers for hands detection
  mframeSize = cv::Size(640, 480);
  mfingersBgr = cv::Mat(mframeSize, CV_8UC3);

  //openni::OpenNI::addDeviceConnectedListener(this);
        //openni::OpenNI::addDeviceDisconnectedListener(this);
        //openni::OpenNI::addDeviceStateChangedListener(this);

  openni::Status rc = openni::STATUS_OK;

  // enumerate all devices
  openni::Array<openni::DeviceInfo> deviceList;
  openni::OpenNI::enumerateDevices(&deviceList);
  const char* deviceURI = openni::ANY_DEVICE;

  if (deviceList.getSize() > mId)
    deviceURI = deviceList[mId].getUri();

  rc = NiDevice.open(deviceURI);
  
  if (rc == openni::STATUS_OK && NiDevice.isValid())
  {
    if (!NiDevice.hasSensor(openni::SENSOR_DEPTH) || !CreateAndStartGenerators())
    {
      NiDevice.close();
    }
    else
    {
      bConnected = true;
      OBJpostEvent(KINECT_CONNECTED_CB, SCOL_PTR this, 0);
    }
  }

  StartThreading(boost::bind(&KinectDevice::GoThread, this));
} 

KinectDevice::KinectDevice() : Thread()
{
}

KinectDevice::~KinectDevice()
{
  StopThreading();

  openni::OpenNI::removeDeviceConnectedListener(this);
        openni::OpenNI::removeDeviceDisconnectedListener(this);
        openni::OpenNI::removeDeviceStateChangedListener(this);

  if(bConnected)
  {
    bConnected = false;

    StopAndDestroyGenerators();
    NiDevice.close();

    //reset users Id
    KinectUserList::iterator iSearchedUser = listOfUsers.begin();
    while (iSearchedUser != listOfUsers.end())
    {
      (*iSearchedUser)->SetId(-1);
      (*iSearchedUser)->SetCalibrated(false);
      iSearchedUser++;
    }
  }
}

void KinectDevice::onDeviceStateChanged(const openni::DeviceInfo* pInfo, openni::DeviceState state)
{
  MMechostr(MSKDEBUG, ">> Kinect state changed");
}

void KinectDevice::onDeviceConnected(const openni::DeviceInfo* pInfo)
{
  MMechostr(MSKDEBUG, ">> Kinect connected");
}

void KinectDevice::onDeviceDisconnected(const openni::DeviceInfo* pInfo)
{
  // just disconnected
  boost::mutex::scoped_lock l(enumerateMutex);
  boost::mutex::scoped_lock lg(uGeneratorsCreate);
  if (bConnected)
  {
    bConnected = false;
    StopAndDestroyGenerators();

    {
      boost::mutex::scoped_lock l(uListMutex);
      KinectUserList::iterator iSearchedUser = listOfUsers.begin();
      while (iSearchedUser != listOfUsers.end())
      {
        (*iSearchedUser)->SetId(-1);
        (*iSearchedUser)->SetCalibrated(false);
        iSearchedUser++;
      }
    }

    NiDevice.close();    
    OBJpostEvent(KINECT_DISCONNECTED_CB, SCOL_PTR this, 0);
  }
  MMechostr(MSKDEBUG, ">> Kinect disconnected");
}

openni::Device& KinectDevice::GetDevice()
{
  assert(&NiDevice);
  return NiDevice;
}

void KinectDevice::GetGeneratorsSize(int &width, int &height)
{
  width = mframeSize.width;
  height = mframeSize.height;
}

void KinectDevice::SetGeneratorsSize(int width, int height)
{
  mframeSize = cv::Size(width, height);
  mfingersBgr = cv::Mat(mframeSize, CV_8UC3);
}

bool KinectDevice::IsConnected()
{
  if (!IsRunning())
    return false;
  else
    return bConnected;
}

void KinectDevice::SetMirrorMode(bool state)
{
  bMirror = state;
  if (mDepthGenerator)
    mDepthGenerator->SetMirror(state);
  if (mImageGenerator)
    mImageGenerator->SetMirror(state);
}

bool KinectDevice::GetMirrorMode()
{
  return bMirror;
}

void KinectDevice::DestroyKinectUser(KinectUser* existingKinectUser)
{
  boost::mutex::scoped_lock l(uListMutex);
  boost::mutex::scoped_lock lh(updateHandsMutex);
  listOfUsers.remove(existingKinectUser);
  SAFE_DELETE(existingKinectUser);
}

KinectUser* KinectDevice::CreateKinectUser()
{
  boost::mutex::scoped_lock l(uListMutex);
  boost::mutex::scoped_lock lh(updateHandsMutex);
  KinectUser* newKinectUser = new KinectUser(this);
  listOfUsers.push_back(newKinectUser);
  return newKinectUser;
}

KinectUser* KinectDevice::GetUser(int userId)
{
  KinectUser* userFound = 0;
  KinectUserList::iterator iSearchedUser = listOfUsers.begin();
  while ((iSearchedUser != listOfUsers.end()) && (userFound == 0))
  {
    if ((*iSearchedUser)->GetId() == userId)
      userFound = *iSearchedUser;
    
    iSearchedUser++;
  }
  return userFound;
}

void KinectDevice::GetValidUsersMap(KinectUser** tUsers)
{
  boost::mutex::scoped_lock l(uListMutex);
  KinectUserList::iterator iSearchedUser = listOfUsers.begin();
  while (iSearchedUser != listOfUsers.end())
  {
    int uid = (*iSearchedUser)->GetId();
    if ((uid > 0) && (uid < 64))
      tUsers[uid] = *iSearchedUser;
    
    iSearchedUser++;
  }
}

void KinectDevice::GoThread()
{
  while(IsRunning())
  {
    if(!bConnected)
    {
      boost::mutex::scoped_lock l(enumerateMutex);
      openni::Status rc = openni::STATUS_OK;

      // enumerate all devices
      openni::Array<openni::DeviceInfo> deviceList;
      openni::OpenNI::enumerateDevices(&deviceList);
      const char* deviceURI = openni::ANY_DEVICE;

      if (deviceList.getSize() > mId)
        deviceURI = deviceList[mId].getUri();

      rc = NiDevice.open(deviceURI);
      
      if (rc == openni::STATUS_OK && NiDevice.isValid())
      {
        if (!NiDevice.hasSensor(openni::SENSOR_DEPTH) || !CreateAndStartGenerators())
        {
          NiDevice.close();
        }
        else
        {
          bConnected = true;
          OBJpostEvent(KINECT_CONNECTED_CB, SCOL_PTR this, 0);
        }
      }
      else
      {
        // wait before retry
        Sleep(200);
      }
    }
    else
    {
      int tick = GetTickCount();
      {
        boost::mutex::scoped_lock l(updateMutex);
        if (mImageGenerator && mImageGenerator->UpdateData())
        {
          if (mUserGenerator && mDepthGenerator)
          {
            boost::mutex::scoped_lock l(uListMutex);
            if (mDepthGenerator->UpdateData(mUserGenerator->UpdateData()))
            {
              DetectUsers();
              DetectHands();
            }
          }
        }
      }

      // limit the update at 60 fps to avoid too fast callbacks
      int elapsedTime = 16 - (GetTickCount() - tick);
      if (elapsedTime > 0)
        Sleep(elapsedTime);
    }
  }
}

unsigned int KinectDevice::GetId()
{
  return mId;
}

int KinectDevice::GetExistingAvailableId()
{
  int found = -1;
  OpenNIUserList::iterator iuser = listOfAvalaibleUsers.begin();
  if (iuser != listOfAvalaibleUsers.end())
  {
    found = (int)*iuser;
    listOfAvalaibleUsers.erase(iuser);
  }
  
  return found;
}

void KinectDevice::AddExistingId(unsigned int id)
{
  OpenNIUserList::iterator iuser = listOfAvalaibleUsers.find(id);
  if (iuser == listOfAvalaibleUsers.end())
  {
    listOfAvalaibleUsers.insert(id);
  }
}

void KinectDevice::RemoveExistingId(unsigned int id)
{
  OpenNIUserList::iterator iuser = listOfAvalaibleUsers.find(id);
  if (iuser != listOfAvalaibleUsers.end())
  {
    listOfAvalaibleUsers.erase(iuser);
  }
}

bool KinectDevice::CreateAndStartGenerators()
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  SAFE_DELETE(mDepthGenerator);
  SAFE_DELETE(mImageGenerator);
  SAFE_DELETE(mUserGenerator);

  //test if generator is ready
  mImageGenerator = new Image(this);
  
  //not ready
  if (!mImageGenerator->UpdateData())
  {
    SAFE_DELETE(mImageGenerator);
    return false;
  }

  SAFE_DELETE(mImageGenerator);

  NiDevice.setDepthColorSyncEnabled(true);

  // Create generators
  mUserGenerator = new User(this);

  //create them after User which define the device image size
  mImageGenerator = new Image(this);
  mDepthGenerator = new Depth(this);
  
  //init smoothing
  mUserGenerator->SetSmoothing(msklSmoothing);

  //init cutoff
  mDepthGenerator->SetDistCutoff(iDistCutoff);
  mDepthGenerator->SetAngCutoff(fAngCutoff);

  if (NiDevice.isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
    NiDevice.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
  
  SetMirrorMode(bMirror);
  return true;
}

void KinectDevice::StopAndDestroyGenerators()
{
  // Stop all the generators
  boost::mutex::scoped_lock l(uGeneratorsCreate);

  SAFE_DELETE(mDepthGenerator);
  SAFE_DELETE(mImageGenerator);
  SAFE_DELETE(mUserGenerator);
  mfingersBgr.setTo(0);
  listOfAvalaibleUsers.clear();
}

Depth* KinectDevice::GetDepthGenerator()
{
  return mDepthGenerator;
}

Image* KinectDevice::GetImageGenerator()
{
  return mImageGenerator;
}

User* KinectDevice::GetUserGenerator()
{
  return mUserGenerator;
}

void KinectDevice::SetSkeletonSmoothing(float value)
{
  boost::mutex::scoped_lock l(updateMutex);
  msklSmoothing = value;
  
  if (mUserGenerator && mUserGenerator->IsValid())
    mUserGenerator->SetSmoothing(msklSmoothing);
}

float KinectDevice::GetSkeletonSmoothing()
{
  return msklSmoothing;
}

void KinectDevice::SetDistCutoff(int dist)
{
  boost::mutex::scoped_lock l(updateMutex);
  iDistCutoff = dist;
  if (mDepthGenerator)
    mDepthGenerator->SetDistCutoff(dist);
}

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

void KinectDevice::SetAngCutoff(float ang)
{
  boost::mutex::scoped_lock l(updateMutex);
  fAngCutoff = ang;
  if (mDepthGenerator)
    mDepthGenerator->SetAngCutoff(ang);
}

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

void KinectDevice::DetectHands()
{
  if (mDepthGenerator->IsValid())
  {
    cv::Mat mat(mframeSize, CV_16SC1, (unsigned char*)mDepthGenerator->GetBuffer());
    cv::Mat matRGB(mframeSize, CV_8UC3);
    matRGB.setTo(0);

    //cv::Mat rbgmat(mframeSize, CV_8UC3, (unsigned char*) GetImageGenerator()->GetBuffer());
    //rbgmat.copyTo(mfingersBgr);
    
    //mfingersDepth.convertTo(mfingersDepth8, CV_8UC1, 255.0f / 3000.0f);
    
    //reset bgr
    //cvtColor(mfingersDepth8, mfingersBgr, CV_GRAY2BGR);
    
    {
      boost::mutex::scoped_lock l(updateHandsMutex);
      KinectUserList::iterator iSearchedUser = listOfUsers.begin();
      while (iSearchedUser != listOfUsers.end())
      {
        if ((*iSearchedUser)->GetId() != -1)
          (*iSearchedUser)->DetectUserHands(mat, matRGB);
        
        iSearchedUser++;
      }
    }

    boost::mutex::scoped_lock lf(uFingersUpdate);
    {      
      matRGB.copyTo(mfingersBgr);
    }
  }
}

void KinectDevice::DetectUsers()
{
  if (!mUserGenerator || !mDepthGenerator)
    return;

  int ucnt = 0;
  int ucntUser = 0;
  
  KinectUser* user = 0;
 
  user = GetUser(-1);

  //attribute an available user Id if possible
  //get user pixels and depth pixels
  if (user && mUserGenerator->IsValid() && mDepthGenerator->IsValid())
  {
    if (listOfAvalaibleUsers.size() > 0)
    {
      const nite::UserId* ubuf = mUserGenerator->GetBuffer();
      const unsigned short* dbuf = mDepthGenerator->GetCuttedBuffer();

      int maxcnt = 0;
      int bestid = 0;
      if ((ubuf != 0) && (dbuf != 0))
      {
        OpenNIUserList::iterator iuser = listOfAvalaibleUsers.begin();
        int width = 0;
        int height = 0;
        mDepthGenerator->GetDepthSize(width, height);

        //find the most visible user in depth
        while ((iuser != listOfAvalaibleUsers.end()) && (user != 0))
        {
          ucnt = 0;
          int uavailable = (int)*iuser;
          
          //count user pixels
          for (int i=0; i < (width * height); i++)
          {
            if (ubuf[i] == uavailable)
            {
              if ((ubuf[i] * dbuf[i]) > 0)
                ucnt++;
            }
          }

          if ((ucnt > 0) && (ucnt > maxcnt))
          {
            maxcnt = ucnt;
            bestid = uavailable;
          }

          iuser++;
        }
      }

      if (bestid != 0)
      {
        RemoveExistingId(bestid);
        user->SetId(bestid);
        OBJpostEvent(KINECT_NEW_USER_CB, (int)user, 0);
      }
    }
  }
}

void KinectDevice::GetHandsPixels(PtrObjBitmap Bcolor, PtrObjBitmap Balpha)
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  if(mDepthGenerator)
  {
    //Scol bitmap are 32bits aligned
    int sbpl = Bcolor->TailleW * 3;
    int dbpl = Bcolor->BPL;

    cv::Mat mat(mframeSize, CV_8UC3);
    {
      try
      {
        boost::mutex::scoped_lock lf(uFingersUpdate);
        mfingersBgr.copyTo(mat);
      }
      catch(std::exception &e)
      {
        MMechostr(MSKDEBUG, "KinectDevice::GetHandsPixels : %s", e.what());
      }
    }

    if (mframeSize != cv::Size(Bcolor->TailleW, Bcolor->TailleH))
      return;

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

          Bcolor->bits[destByte] = mat.data[srcByte];
          Bcolor->bits[destByte + 1] = mat.data[srcByte + 1];
          Bcolor->bits[destByte + 2] = mat.data[srcByte + 2];
        }
      }

    if (Balpha)
    {
      int dabpl = Balpha->BPL;

      //copy alpha
      for (long y=0; y<Balpha->TailleH; y++)
      {
        for (long x=0; x<Balpha->TailleW; x++)
        {
          unsigned long sr = mat.data[(x * 3) + (sbpl * y)];
          unsigned long sg = mat.data[((x * 3) + (sbpl * y)) + 1];
          unsigned long sb = mat.data[((x * 3) + (sbpl * y)) + 2];
          unsigned long destByte = (x * Balpha->BytesPP) + (dabpl * y);

          Balpha->bits[destByte] = (sr + sg + sb) == 0 ? 0 : 0xFF;
        }
      }
    }
  }
}

void KinectDevice::GetDepthBuffer(PtrObjBitmap Bcolor)
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  if(mDepthGenerator)
    mDepthGenerator->GetBuffer(Bcolor);
}

void KinectDevice::GetImageBuffer(PtrObjBitmap Bcolor)
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  if(mImageGenerator)
    mImageGenerator->GetBuffer(Bcolor);
}

void KinectDevice::GetImageGreyBuffer(PtrObjBitmap Bcolor)
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  if(mImageGenerator)
    mImageGenerator->GetGreyBuffer(Bcolor);
}

void KinectDevice::GetValidUserPixels(PtrObjBitmap Bcolor, PtrObjBitmap Balpha)
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  if(mUserGenerator)
    mUserGenerator->GetValidUserPixels(Bcolor, Balpha);
}

void KinectDevice::GetValidUserPixelsRGB(PtrObjBitmap Bcolor, PtrObjBitmap Balpha)
{
  boost::mutex::scoped_lock l(uGeneratorsCreate);
  if(mUserGenerator)
    mUserGenerator->GetValidUserPixels(Bcolor, Balpha);

  if(mImageGenerator)
    mImageGenerator->GetBuffer(Bcolor);
}

bool KinectDevice::GetSklMutexLock()
{
  return uSklMutex.try_lock();
}

void KinectDevice::ReleaseSklMutex()
{
  uSklMutex.unlock();
}