User.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/User.h"
#include "DeviceManager.h"
#include "objects/KinectDevice.h"
#include "objects/KinectUser.h"
#include "openNiScolPlugin.h"

User::User(KinectDevice* pDevice)
{
  width = 0;
  height = 0;
  mParentDevice = pDevice;
  mData = 0;
  mReadData = 0;
  mUserTracker = new nite::UserTracker;

  openni::Device& dev = mParentDevice->GetDevice();
  if (mUserTracker->create(&dev) == nite::STATUS_OK)
  {
    // update the generator sizes with the depth size
    if (mUserTracker->readFrame(&mTrackerFrame) == nite::STATUS_OK)
    {
      width = mTrackerFrame.getDepthFrame().getWidth();
      height = mTrackerFrame.getDepthFrame().getHeight();
      mParentDevice->SetGeneratorsSize(width, height);
    }
    else
      pDevice->GetGeneratorsSize(width, height);

    mData = (nite::UserId*)malloc(width * height * sizeof(nite::UserId));
    memset(mData, 0, width * height * sizeof(nite::UserId));
    
    mReadData = (nite::UserId*)malloc(width * height * sizeof(nite::UserId));
    memset(mReadData, 0, width * height * sizeof(nite::UserId));
  }
  else
  {
    MMechostr(MSKDEBUG, "OpenNI device : could not create User generator\n");
  }
}

User::User()
{
}

User::~User()
{
  boost::mutex::scoped_lock l(mmutex);
  if (mData)
    free(mData);

  if (mReadData)
    free(mReadData);

  mTrackerFrame.release();
  delete(mUserTracker);
}

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

nite::UserTracker* User::GetUserTraker()
{
  return mUserTracker;
}

nite::UserTrackerFrameRef* User::GetTrakerFrame()
{
  return &mTrackerFrame;
}

openni::VideoFrameRef User::UpdateData()
{
  boost::mutex::scoped_lock l(mmutex);

  openni::VideoFrameRef depthFrame;
  if (mParentDevice->GetDevice().isValid() && mData && mUserTracker->isValid())
  {
    if (mUserTracker->readFrame(&mTrackerFrame) != nite::STATUS_OK)
                  return depthFrame;

    depthFrame = mTrackerFrame.getDepthFrame();

    const nite::UserMap& userLabels = mTrackerFrame.getUserMap();
    const nite::UserId* pLabels = userLabels.getPixels();

    memcpy(mData, pLabels, width * height * sizeof(nite::UserId));

    const nite::Array<nite::UserData>& users = mTrackerFrame.getUsers();
          for (int i = 0; i < users.getSize(); ++i)
          {
                  const nite::UserData& user = users[i];
      KinectUser* kuser = mParentDevice->GetUser(user.getId());

                  //updateUserState(user, userTrackerFrame.getTimestamp());
                  if (user.isNew())
                  {
        mParentDevice->AddExistingId(user.getId());
                  }
                  else if (user.isLost())
                  {
        mParentDevice->RemoveExistingId(user.getId());
  
        if(kuser)
        {
          //User update is done in thread loop
          kuser->SetId(-1);
          kuser->SetCalibrated(false);
          OBJpostEvent(KINECT_LOST_USER_CB, SCOL_PTR kuser, 0);
        }
                  }
      else if (user.isVisible())
      {
        if (users[i].getSkeleton().getState() == nite::SKELETON_NONE && kuser)
                          {
          if (mUserTracker->startSkeletonTracking(user.getId()) == nite::STATUS_OK)
            OBJpostEvent(KINECT_USER_CALIBRATION_START_CB, SCOL_PTR kuser, 0);
                          }
        else if (kuser && !kuser->IsCalibrated() && users[i].getSkeleton().getState() == nite::SKELETON_TRACKED)
        {
          kuser->SetCalibrated(true);
          OBJpostEvent(KINECT_USER_CALIBRATION_END_CB, SCOL_PTR kuser, 0);
        }
      }
          }
  }
  return depthFrame;
}

bool User::IsValid()
{
  return mUserTracker->isValid();
}

const nite::UserId* User::GetBuffer()
{
  return mData;
}

void User::SetSmoothing(float sklSmoothing)
{
  if (mUserTracker->isValid())
    mUserTracker->setSkeletonSmoothingFactor(sklSmoothing);
}

void User::GetValidUserPixels(PtrObjBitmap Bcolor, PtrObjBitmap Balpha)
{
  // Get buffer
  {
    boost::mutex::scoped_lock l(mmutex);
    {
      if (mData && mReadData)
        memcpy(mReadData, mData, width * height * sizeof(nite::UserId));
      else
        return;
    }
  }
  
  if ((Bcolor->TailleW != width) || (Bcolor->TailleH != height))
    return;
  
  if ((Balpha->TailleW != width) || (Balpha->TailleH != height))
    return;
  
  bool isMirror = GetParentDevice()->GetMirrorMode();
  
  //using GetUser(id) per pixel take too long, so we use a temporary tab indexed with ids
  KinectUser* tUsers[64];
  memset(tUsers, 0, sizeof(KinectUser*) * 64);
  GetParentDevice()->GetValidUsersMap(tUsers);

  // Browse all pixels
  long x = 0, y = 0;
  unsigned long srcByte = 0, destByte = 0;
  for (y = 0; y < height; y++)
  {
    for (x = 0; x < width; x++)
    {
      unsigned int fixed_x = x;
            /*if(!isMirror)
            {
                    fixed_x = (rgbW - 1) - x;
            }
      */

      srcByte = y*width + fixed_x;
      destByte = x*3 + y*width*3;
      
      int id = mReadData[srcByte];

      int color = 0;

      KinectUser* user = 0;
      if (id < 64)
      {
        user = tUsers[id];
        if (user != 0)
          color = user->GetColor();
      }

      Bcolor->bits[destByte] = color &0xFF;
      Bcolor->bits[destByte+1] = color>>8 &0xFF;
      Bcolor->bits[destByte+2] = color>>16 &0xFF;
      Balpha->bits[(x + y*width)] = (user == 0) || (id == 0) ? 0 : 0xFF;
    }
  }
}

void User::stopTracking(KinectUser* user)
{
  if(user->GetId() > 0 && mUserTracker->isValid())
  {
    mUserTracker->stopSkeletonTracking(user->GetId());
  }
}

void User::startTracking(KinectUser* user)
{
  if(user->GetId() > 0 && mUserTracker->isValid())
  {
    mUserTracker->startSkeletonTracking(user->GetId());
  }
}

/*
void User::registerCallbacks()
{
  if (IsValid())
  {
    xn::SkeletonCapability skl = GetSkeletonCap();
    skl.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
    skl.SetSmoothing(0.6f);
    RegisterUserCallbacks(NewUser, LostUser, GetParentDevice(), mhUserCallbacks);
    skl.RegisterToCalibrationStart(CalibrationStart, GetParentDevice(), mhCalibrationStart);
    skl.RegisterToCalibrationComplete(CalibrationEnd, GetParentDevice(), mhCalibrationEnd);
  }
}

void User::unRegisterCallbacks()
{
  if (IsValid())
  {
    if (mhUserCallbacks)
    {
      UnregisterUserCallbacks(mhUserCallbacks);
      mhUserCallbacks = 0;
    }
    if(mhCalibrationStart)
    {
      GetSkeletonCap().UnregisterFromCalibrationStart(mhCalibrationStart);
      mhCalibrationStart = 0;
    }

    if(mhCalibrationEnd)
    {
      GetSkeletonCap().UnregisterFromCalibrationComplete(mhCalibrationEnd);
      mhCalibrationEnd = 0;
    }
  }
}
*/