common.h

/*
-----------------------------------------------------------------------------
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

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

#ifndef __COMMON_H__
#define __COMMON_H__

//---------------------------------------------------------------------------
// Includes
//---------------------------------------------------------------------------

// OpenCV inlude
#include <opencv2/opencv.hpp>

// Scol SDK include
#include <ScolPlugin.h>
//#include <OS_specific/windows/Objstr.h>

#include "stdint.h"

// OpenNI include
#include <OpenNI.h>
#include <NiTE.h>

// Standard C++ includes
// includes
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>   
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>
#include <string>
#include <iostream>
#include <map>
#include <list>
#include <set>
#include <sstream>
#include <tchar.h>
#include <math.h>

// Standard C includes
#include <cassert>

#define SCOL_PI       3.14159265358979323846f

// Standard C++ namespace
using namespace std;

//---------------------------------------------------------------------------
// Class pre-declarations
//---------------------------------------------------------------------------
template <class DERIVATED_CLASS> class Singleton;
class Thread;
class DataSkeleton;
class Depth;
class Image;
class User;
class KinectDevice;
class KinectUser;
class KinectGesture;

// Base class managing all the devices
class DeviceManager;


//---------------------------------------------------------------------------
// Boost Map pre-declarations
//---------------------------------------------------------------------------
typedef std::list<KinectUser*> KinectUserList;
typedef std::set<unsigned int> OpenNIUserList;
typedef std::map<unsigned int, KinectDevice*> KinectDeviceMap;


class Quaternion
{
  public:
    Quaternion()
    {
      x = 0.0f;
      y = 0.0f;
      z = 0.0f;
      w = 1.0f;
    }
  
          float x;
    float y;
    float z;
    float w;
};

class Matrix3
{
  public:

  Matrix3 (float fEntry00, float fEntry01, float fEntry02,
                  float fEntry10, float fEntry11, float fEntry12,
                  float fEntry20, float fEntry21, float fEntry22)
  {
          m[0][0] = fEntry00;
          m[0][1] = fEntry01;
          m[0][2] = fEntry02;
          m[1][0] = fEntry10;
          m[1][1] = fEntry11;
          m[1][2] = fEntry12;
          m[2][0] = fEntry20;
          m[2][1] = fEntry21;
          m[2][2] = fEntry22;
  }


  // member access, allows use of construct mat[r][c]
  inline float* operator[] (size_t iRow) const
  {
    return (float*)m[iRow];
  }

  bool operator== (const Matrix3& rkMatrix) const;
  inline bool operator!= (const Matrix3& rkMatrix) const
        {
                return !operator==(rkMatrix);
        }

  // arithmetic operations
  Matrix3 operator+ (const Matrix3& rkMatrix) const;
  Matrix3 operator- (const Matrix3& rkMatrix) const;
  Matrix3 operator* (const Matrix3& rkMatrix) const;
  Matrix3 operator- () const;

  // matrix * vector [3x3 * 3x1 = 3x1]
  //Vector3 operator* (const Vector3& rkVector) const;

  protected:
    float m[3][3];
};

class cvTools
{
  public:
  static void DrawContour(cv::Mat &img, const vector<cv::Point> &contour, const cv::Scalar &color) {
          vector<vector<cv::Point>> contours;
          contours.push_back(contour);
          drawContours(img, contours, -1, color);
  }

  static float getAngle(cv::Point i, cv::Point j, cv::Point k)
  {
    float x1 = static_cast<float>(i.x);
    float y1 = static_cast<float>(i.y);
    float x2 = static_cast<float>(j.x);
    float y2 = static_cast<float>(j.y);
    float x3 = static_cast<float>(k.x);
    float y3 = static_cast<float>(k.y);
    float d12 = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1);
    float d13 = (x3-x1)*(x3-x1) + (y3-y1)*(y3-y1);
    float d23 = (x2-x3)*(x2-x3) + (y2-y3)*(y2-y3);
    float theta = acos((d13-d12-d23)/(-2*sqrt(d12)*sqrt(d23)));
    return theta * 180.0f / 3.14159265f;
  }

  static cv::Point2i getCentroid(cv::Point i, cv::Point j, cv::Point k)
  {
    int x1 = i.x;
    int y1 = i.y;
    int x2 = j.x;
    int y2 = j.y;
    int x3 = k.x;
    int y3 = k.y;
    int x = (x1+x2+x3)/3;
    int y = (y1+y2+y3)/3;
    return cv::Point2i(x, y);
  }

  static double Convexity(const vector<cv::Point> &contour) {
          cv::Mat contourMat(contour);

          vector<int> hull;
          cv::convexHull(contourMat, hull);

          int n = hull.size();
          vector<cv::Point> hullContour;

          for (int i=0; i<n; i++) {
                  hullContour.push_back(contour[hull[i]]);
          }

          cv::Mat hullContourMat(hullContour);

    return (cv::contourArea(contourMat) / cv::contourArea(hullContourMat));
  }

  static void MatAverage(vector<cv::Mat>& frames, cv::Mat& mean)
  {
          cv::Mat1d acc(mean.size());
          cv::Mat1d frame(mean.size());

          for (unsigned int i=0; i<frames.size(); i++)
    {
            frames[i].convertTo(frame, CV_64FC1);
            acc = acc + frame;
          }

          acc = acc / frames.size();

          acc.convertTo(mean, CV_16SC1);
  }

  static void VecAverage(vector<vector<cv::Point>>& dots, vector<cv::Point>& mean)
  {
    vector<cv::Point> acc;
    vector<int> countacc;

          for (unsigned int i=0; i<dots.size(); i++)
    {
            for (unsigned int j=0; j<dots[i].size(); j++)
      {
        if (j >= acc.size())
        {
          acc.push_back(dots[i][j]);
          countacc.push_back(1);
        }
        else
        {
          acc[j] = cv::Point(dots[i][j].x + acc[j].x, dots[i][j].y + acc[j].y);
          countacc[j] = countacc[j] + 1;
        }
            }
          }

    for (unsigned int i=0; i<acc.size(); i++)
    {
      acc[i] = cv::Point((acc[i].x / countacc[i]), (acc[i].y / countacc[i]));
    }

    mean = acc;
  }

  static float sqr(float a) 
  {
    return a * a;
  }

  static void OrderPoints(vector<vector<cv::Point>>& dots, vector<vector<cv::Point>>& mean)
  {
    vector<vector<cv::Point>> cdots = dots;
    mean = cdots;
/*
    if (cdots.size() > 1)
    {
      //TODO get the largest list, fill holes with current coords
      vector<cv::Point> vRef = cdots[0];

            for (unsigned int i=1; i<cdots.size(); i++)
      {
              for (unsigned int j=0; j<vRef.size(); j++)
        {
          int minDist = -1;
          int minPos = 0;
          cv::Point refDot = vRef[j];
          
          for (unsigned int k=0; k<cdots[i].size() && j<cdots[i].size(); k++)
          {
            int ddist = sqrt(sqr(cdots[i][k].x - refDot.x) + sqr(cdots[i][k].y - refDot.y));
            if ((ddist < minDist) || (minDist == -1))
            {
              minDist = ddist;
              minPos = k;
            }
          }
          
          if ((minDist != -1) && (minPos != j))
          {
            mean[i].insert(mean[i].begin() + j, mean[i][minPos]);
            mean[i].erase(mean[i].begin() + minPos);
            j = cdots[i].erase(dots[i].begin() + minPos);
          }

              }
            }
    }*/
  }

};

#endif