KinectUserHand.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/KinectUserHand.h"
#include "core/DataSkeleton.h"
#include "DeviceManager.h"
#include "generator/User.h"
#include "objects/KinectDevice.h"
#include "openNiScolPlugin.h"

// Constructors
KinectUserHand::KinectUserHand()
{
}

KinectUserHand::KinectUserHand(KinectUser* user, nite::JointType type)
{
  muser = user;
  mtype = type;
  mvisible = false;
}

// Destructor
KinectUserHand::~KinectUserHand()
{
}

bool KinectUserHand::GetHandContour(const cv::Mat &depthMat, cv::Mat &handMat, nite::Point3f v, vector<cv::Point> &handContour, cv::Point2f &center, cv::Mat *debugFrame)
{
  const float handDepthRange = 300.0f; // in mm
  const float depth = v.z; // hand depth
  const int maxHandRadius = ((depth / 100.0f) > 0) ? (int) (700.0f / (depth / 100.0f)) : 128; // in px
  const float dnear = depth - (handDepthRange / 2); // near clipping plane
  const float dfar = depth + (handDepthRange / 2); // far clipping plane
  
  const double erodeCoef = depth > 0 ? 30.0f / (depth / 100) : 30.0f;
  const double blurCoef = (depth / 100) <= 70 ? 11.0f : (depth / 100) <= 90 ? 6.0f : 2.0f;
  const double epsilon = depth > 0 ? 65.0f / (depth / 100) : 65.0f; //17.6; // approximation accuracy (maximum distance between the original hand contour and its approximation)

  handMat.setTo(0);

  // extract hand region
  cv::circle(handMat, cv::Point((int)v.x, (int)v.y), maxHandRadius, 255, CV_FILLED);
  handMat = handMat & depthMat > dnear & depthMat < dfar;

  //remove noise
  try
  {
    //erode picture to remove noise
    //cv::GaussianBlur(handMat, handMat, cv::Size(blurCoef, blurCoef), 1, 2);
    cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size((int)(2.0f * erodeCoef + 1.0f), (int)(2.0f * erodeCoef + 1.0f)), cv::Point((int)erodeCoef, (int)erodeCoef));
    cv::erode(handMat, handMat, element);
  }
  catch(cv::Exception)
  {
  }

  if (debugFrame)
  {
    int width;
    int height;
    muser->GetParentDevice()->GetGeneratorsSize(width, height);

    cv::Mat maskrgb(cv::Size(width, height), CV_8UC3);
    cvtColor(handMat, maskrgb, CV_GRAY2BGR);
    cv::add(maskrgb, *debugFrame, *debugFrame);
  }

  // assume largest contour in hand region to be the hand contour
  vector<vector<cv::Point>> contours;
  
  cv::findContours(handMat, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cv::Point(0, 20));
  int n = contours.size();
  int maxI = -1;
  int maxSize = -1;
  for (int i=0; i<n; i++)
  {
    int size = contours[i].size();
    if (size > maxSize)
    {
            maxSize = size;
            maxI = i;
    }
  }

  //handContourFound
  if (maxI >= 0)
  {
    vector<cv::Point> tHandContour = contours[maxI];
    cv::approxPolyDP(cv::Mat(contours[maxI]), tHandContour, epsilon, true);

    //real hand center
    float radius;
    cv::minEnclosingCircle(tHandContour, center, radius);
    handContour = tHandContour;

    if (debugFrame)
    {
      // hand angle
      cv::Point2f recPoints[4];
      cv::RotatedRect shapeRect = cv::fitEllipse(tHandContour);
      shapeRect.points(recPoints);

      cv::circle(*debugFrame, cv::Point((int)center.x, (int)center.y), (int)radius, 0x00ff00, 1);
      cv::line(*debugFrame, center, cv::Point((int)(center.x + (cos(SCOL_PI * shapeRect.angle / 180.0f) * radius)), (int)(center.y + (sin(SCOL_PI * shapeRect.angle / 180.0f) * radius))), 0x0000ff, 2);
      for (int j=0; j<4; j++)
      {
        if (j != 0)
        {
                cv::line(*debugFrame, recPoints[j], recPoints[j-1], 0x00ff00, 2);
        }
        else
        {
                cv::line(*debugFrame, recPoints[0], recPoints[3], 0x00ff00, 2);
        }
      }

      for (int j=0; j<(int)handContour.size(); j++)
      {
        if (j != 0)
        {
                cv::line(*debugFrame, handContour[j], handContour[j-1], 128, 2);
        }
        else
        {
                cv::line(*debugFrame, handContour[0], handContour[handContour.size()-1], 128, 2);
        }
      }
      cv::circle(*debugFrame, cv::Point((int)v.x, (int)v.y), maxHandRadius, 255, 2);
    }
  }

  return (maxI >= 0);
}

void KinectUserHand::DetectFingerTips(cv::Mat &handMat, vector<cv::Point> &handContour, cv::Point2f &center, cv::Mat *debugFrame)
{
  cv::Mat handContourMat(handContour);
  const cv::Scalar debugFingerTipColor(0, 0, 255);

  int width = 0;
  int height = 0;
  muser->GetParentDevice()->GetGeneratorsSize(width, height);

  vector<int> hull;
  vector<cv::Point> tmpFingerTips;

  cv::convexHull(handContourMat, hull, true, false);

  // find interior angles of hull corners
  for (int j=0; (j<(int)hull.size()) && (tmpFingerTips.size() < 5); j++)
  {
    int idx = hull[j]; // corner index
    int pdx = idx == 0 ? handContour.size() - 1 : idx - 1; //  predecessor of idx
    int sdx = idx == handContour.size() - 1 ? 0 : idx + 1; // successor of idx

    cv::Point v1 = handContour[sdx] - handContour[idx];
    cv::Point v2 = handContour[pdx] - handContour[idx];

    float angle = acos((float)((v1.x*v2.x + v1.y*v2.y) / (norm(v1) * norm(v2))));

    // low interior angle + within upper 90% of region -> we got a finger
    if (angle < 1.0f)
    //&& handContour[idx].y < cutoff
    {
            int u = handContour[idx].x;
            int v = handContour[idx].y;

      tmpFingerTips.push_back(cv::Point2i(u, v));
    }
  }

  // The K-curvature algorithm
  /*
  int curv = 20;
  int contourRadius = 15;
  for (std::vector<cv::Point>::iterator it = handContour.begin(); it != handContour.end(); ++it)
  {
    cv::Point i = *it;
    std::vector<cv::Point>::iterator it1 = it;
    for (int counter = 0; it1 != handContour.end() && counter < curv; ++it1, ++counter);
    if (it1 != handContour.end())
    {
      cv::Point j = *it1;
      std::vector<cv::Point>::iterator it2 = it1;
      for (int counter = 0; it2 != handContour.end() && counter < curv; ++it2, ++counter);
      if (it2 != handContour.end())
      {
        cv::Point k = *it2;
        float angle = cvTools::getAngle(i, j, k);
        if (angle > 25.0f && angle < 35.0f)
        {
          cv::Point2i center = cvTools::getCentroid(i, j, k);
          for (int counter = 0; it != handContour.end() && counter < curv; ++it, ++counter);

          for(int l=0; l<hull.size(); l++)
          {
            cv::Point pt = handContour[hull[l]];
            if(pt.y - contourRadius <= k.y && pt.y + contourRadius >= k.y && pt.x - contourRadius <= k.x && pt.x + contourRadius >= k.x)
            {
              tmpFingerTips.push_back(k);
            }
          }
        }
      }
    }
  }*/

  //rotate the vector
  if(mfingerTipsHistory.size() > 5)
    mfingerTipsHistory.erase(mfingerTipsHistory.begin());

  mfingerTipsHistory.push_back(tmpFingerTips);
  std::vector<std::vector<cv::Point>> sortedtips;

  for (int j=0; j<(int)mfingerTipsHistory.size(); j++)
  {
    std::vector<cv::Point> ftips = mfingerTipsHistory[j];
    for (int k=0; k<(int)ftips.size(); k++)
    {
      if (k > ((int)sortedtips.size() - 1))
      {
        std::vector<cv::Point> fv;
        fv.push_back(ftips[k]);
        sortedtips.push_back(fv);
      }
      else
      {
        sortedtips[k].push_back(ftips[k]);
      }
    }
  }

  {
    boost::mutex::scoped_lock l(handMutex);
    //clear the current finger tips coords
    mfingerTips.clear();

    for (int j=0; j<(int)sortedtips.size(); j++)
    {
      cv::Point fvec(0, 0);
      for (int k=0; k<(int)sortedtips[j].size(); k++)
      {
        fvec += sortedtips[j][k];
      }

      if(sortedtips[j].size() > 0)
      {
        fvec.x /= sortedtips[j].size();
        fvec.y /= sortedtips[j].size();
      }

      mfingerTips.push_back(fvec);
    }
  }

  if (debugFrame)
  {
    for (int j=0; j<(int)mfingerTips.size(); j++)
    {
      for (int k=0; k<(int)mfingerTips.size(); k++)
      {
        // draw fingertips
        cv::circle(*debugFrame, mfingerTips[k], 3, debugFingerTipColor, -1);
        cv::line(*debugFrame, center, mfingerTips[k], debugFingerTipColor);
      }
    }
  }
/*
  if (debugFrame)
  {
    // draw cutoff threshold
    cv::line(*debugFrame, cv::Point(0, cutoff), cv::Point(width, cutoff), debugFingerTipColor);

    // draw approxCurve
    for (int j=0; j<(int)handContour.size(); j++)
    {
            cv::circle(*debugFrame, handContour[j], 3, debugFingerTipColor);
            if (j != 0)
      {
                    cv::line(*debugFrame, handContour[j], handContour[j-1], debugFingerTipColor);
            }
      else
      {
                    cv::line(*debugFrame, handContour[0], handContour[handContour.size()-1], debugFingerTipColor);
            }
    }

    // draw approxCurve hull
    for (int j=0; j<(int)hull.size(); j++)
    {
            cv::circle(*debugFrame, handContour[hull[j]], 3, debugFingerTipColor, 3);
            if(j == 0)
      {
                    cv::line(*debugFrame, handContour[hull[j]], handContour[hull[hull.size()-1]], debugFingerTipColor);
            }
      else
      {
                    cv::line(*debugFrame, handContour[hull[j]], handContour[hull[j-1]], debugFingerTipColor);
            }
    }
  }*/
}

bool KinectUserHand::Detect(cv::Mat depthMat, cv::Mat depthMatBgr)
{
  const int minHandExtension = 80; // in milimeter
  const int minTorsoExtension = 250; // in pixel
  const double grabConvexity = 0.8;

  // torso
  nite::Point3f tVec;
  tVec.x = 0;
  tVec.y = 0;
  tVec.z = 0;

  //hand
  nite::Point3f hVec;
  hVec.x = 0;
  hVec.y = 0;
  hVec.z = 0;

  if (muser->GetSkeletonData()->GetBoneImgCoordinates(nite::JOINT_TORSO, tVec, 0.7))
  {
    try
    {
      int width;
      int height;
      muser->GetParentDevice()->GetGeneratorsSize(width, height);

                  // hand
                  if ((muser->GetSkeletonData()->GetBoneImgCoordinates(mtype, hVec, 0.7)) /* confident detection */ && 
                          (hVec.z < tVec.z - minHandExtension) /* user extends hand towards screen */
                          && (hVec.y < (tVec.y + minTorsoExtension))) /* user raises his hand */
      {
                          unsigned char shade = 255 - (unsigned char)((hVec.z / 1000.0f) *  128.0f);
        cv::Scalar color(0, 0, shade);

        vector<cv::Point> handContour;
        cv::Point2f center;
        cv::Mat handMat(cv::Size(width, height), CV_8UC1);
                          if (GetHandContour(depthMat, handMat, hVec, handContour, center, 0/*&depthMatBgr*/))
        {
          if (!mvisible)
          {
            {
              boost::mutex::scoped_lock l(handMutex);
              mvisible = true;
            }
            OBJpostEvent(KINECT_USER_HAND_FOUND_CB, SCOL_PTR muser, (int)mtype);
          }
          
          if(mhandPosHistory.size() > 5)
            mhandPosHistory.erase(mhandPosHistory.begin());

          nite::Point3f hpos;
          hpos.x = center.x;
          hpos.y = center.y;
          hpos.z = hVec.z;

          mhandPosHistory.push_back(hpos);
          nite::Point3f smoothvec;
          smoothvec.x = 0;
          smoothvec.y = 0;
          smoothvec.z = 0;
          for (int j=0; j<(int)mhandPosHistory.size(); j++)
          {
            smoothvec.x += mhandPosHistory[j].x;
            smoothvec.y += mhandPosHistory[j].y;
            smoothvec.z += mhandPosHistory[j].z;
          }

          if(mhandPosHistory.size() > 0)
          {
            smoothvec.x /= mhandPosHistory.size();
            smoothvec.y /= mhandPosHistory.size();
            smoothvec.z /= mhandPosHistory.size();
          }

          {
            boost::mutex::scoped_lock l(handMutex);
            mtransVec.x = smoothvec.x - mhandPos.x;
            mtransVec.y = smoothvec.y - mhandPos.y;
            mtransVec.z = smoothvec.z - mhandPos.z;
            mhandPos = smoothvec;
          }
         
          bool grasp = cvTools::Convexity(handContour) > grabConvexity;
                            int thickness = grasp ? CV_FILLED : 3;
          cv::circle(depthMatBgr, cv::Point((int)mhandPos.x, (int)mhandPos.y), 5, color, thickness);

                            DetectFingerTips(handMat, handContour, center, &depthMatBgr);

          if ((mtransVec.x != 0) || (mtransVec.y != 0) || (mtransVec.z != 0))
            OBJpostEvent(KINECT_USER_HAND_MOVE_CB, SCOL_PTR muser, (int)this);

          return true;
        }
        return false;
                  }
      else
      {
        if (mvisible)
        {
          {
            boost::mutex::scoped_lock l(handMutex);
            mvisible = false;
          }
          OBJpostEvent(KINECT_USER_HAND_LOST_CB, SCOL_PTR muser, (int)mtype);
        }
        return false;
      }
    }
    catch (cv::Exception &e)
    {
      MMechostr(MSKDEBUG, const_cast<char*>(e.what()));
      return false;
    }
  }
  return false;
}

vector<cv::Point> KinectUserHand::GetFingersPos()
{
  boost::mutex::scoped_lock l(handMutex);
  vector<cv::Point> vFingers = mfingerTips;
  return vFingers;
}

nite::Point3f KinectUserHand::GetHandPos()
{
  boost::mutex::scoped_lock l(handMutex);
  nite::Point3f pos = mhandPos;
  return pos;
}

nite::Point3f KinectUserHand::GetLastTransVec()
{
  boost::mutex::scoped_lock l(handMutex);
  nite::Point3f transVec = mtransVec;
  return transVec;
}

bool KinectUserHand::IsVisible()
{
  boost::mutex::scoped_lock l(handMutex);
  return mvisible;
}

nite::JointType KinectUserHand::GetType()
{
  return mtype;
}