Prerequisites.h

Go to the documentation of this file.

/*
-----------------------------------------------------------------------------
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 __BTK_PREREQUISITES_H__
#define __BTK_PREREQUISITES_H__
 
#include <scolPlugin.h>
#include <scolMemoryHelper.hpp>
 
#ifdef _WIN32
#include <tchar.h>
#endif
 
#include <opencv2/opencv.hpp>
#include <math.h>
#include <list>
#include <vector>
#include <string>
#include <boost/filesystem/operations.hpp>
 
// We can safely disable this warning that occurs in aruco library.
#pragma warning(disable : 4290)
 
// MlToolKit variables
#define SCOL_ML_TRAINING_FINISHED_CB 0
extern int WM_ML_TRAINING_FINISHED;
 
#define SCOL_ML_DETECTION_CB 1
extern int WM_ML_DETECTION;
 
#ifdef _DEBUG
//#define _SCOL_DEBUG_
#endif
 
// ToolKit loading functions
int LoadMathToolkit(mmachine m);
int LoadBitmapToolKit(mmachine m);
int     LoadCaptureToolkit(mmachine m);
int LoadArToolkit(mmachine m);
int LoadMlToolkit(mmachine m);
int LoadMediaPlayerToolkit(mmachine m);
int UnloadArToolkit();
int UnloadMediaPlayerToolkit();
 
// disable warning for aruco
#pragma warning(disable : 4290)
 
int Random(int mi, int mx);
 
// Vector of 2 float components
class Vector2
{
public:
  Vector2()
  {
    x = 0.0f;
    y = 0.0f;
  }
 
  Vector2(float xx, float yy)
  {
    x = xx;
    y = yy;
  }
 
  //-----------------------------------------------------------------------
  // Utility
  float GetMagnitude()
  {
    return sqrtf(x*x + y * y);
  }
 
  Vector2 GetNoramlizedVector()
  {
    float magnitude = GetMagnitude();
    if (magnitude == 0.0f)
      return Vector2(0.0f, 0.0f);
    return Vector2(x / magnitude, y / magnitude);
  }
 
  float DotProduct(Vector2 other)
  {
    return x * other.x + y * other.y;
  }
 
  //-----------------------------------------------------------------------
  // Unary operators
  Vector2 operator-()
  {
    return Vector2(-x, -y);
  }
 
  Vector2 operator+=(Vector2 other)
  {
    return Vector2(x + other.x, y + other.y);
  }
 
  Vector2 operator-=(Vector2 other)
  {
    return Vector2(x - other.x, y - other.y);
  }
 
  Vector2 operator*=(float scalar)
  {
    return Vector2(x * scalar, y * scalar);
  }
 
  Vector2 operator/=(float scalar)
  {
    return Vector2(x / scalar, y / scalar);
  }
 
protected:
private:
 
public:
  float x;
  float y;
protected:
private:
};
 
//-----------------------------------------------------------------------
// Vector2 binary operators
inline Vector2 operator+(Vector2 u, Vector2 v)
{
  return Vector2(u.x + v.x, u.y + v.y);
}
 
inline Vector2 operator-(Vector2 u, Vector2 v)
{
  return u + (-v);
}
inline Vector2 operator*(Vector2 u, float scalar)
{
  Vector2 result = u;
  result *= scalar;
  return result;
}
inline Vector2 operator*(float scalar, Vector2 u)
{
  return u * scalar;
}
inline Vector2 operator/(Vector2 u, float scalar)
{
  Vector2 result = u;
  result /= scalar;
  return result;
}
 
// Vector with 3 float components
class Vector3
{
public:
  Vector3()
  {
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
  }
 
  Vector3(float xx, float yy, float zz)
  {
    x = xx;
    y = yy;
    z = zz;
  }
 
  //-----------------------------------------------------------------------
  // Utility
  float GetMagnitude()
  {
    return sqrtf(x*x + y * y + z * z);
  }
 
  Vector3 GetNoramlizedVector()
  {
    float magnitude = GetMagnitude();
    if (magnitude == 0.0f)
      return Vector3(0.0f, 0.0f, 0.0f);
    return Vector3(x / magnitude, y / magnitude, z / magnitude);
  }
 
  float DotProduct(Vector3 other)
  {
    return x * other.x + y * other.y + z * other.z;
  }
 
  Vector3 CrossProduct(Vector3 other)
  {
    return Vector3(y * other.z - z * other.y, z * other.x - x * other.z, x * other.y - y * other.x);
  }
 
  //-----------------------------------------------------------------------
  // Unary operators
  Vector3 operator-()
  {
    return Vector3(-x, -y, -z);
  }
 
  Vector3 operator+=(Vector3 other)
  {
    return Vector3(x + other.x, y + other.y, z + other.z);
  }
 
  Vector3 operator-=(Vector3 other)
  {
    return Vector3(x - other.x, y - other.y, z - other.z);
  }
 
  Vector3 operator*=(float scalar)
  {
    return Vector3(x * scalar, y * scalar, z * scalar);
  }
 
  Vector3 operator/=(float scalar)
  {
    return Vector3(x / scalar, y / scalar, z / scalar);
  }
 
protected:
private:
 
public:
  float x;
  float y;
  float z;
protected:
private:
};
 
//-----------------------------------------------------------------------
// Vector3 binary operators
inline Vector3 operator+(Vector3 u, Vector3 v)
{
  return Vector3(u.x + v.x, u.y + v.y, u.z + v.z);
}
 
inline Vector3 operator-(Vector3 u, Vector3 v)
{
  return u + (-v);
}
inline Vector3 operator*(Vector3 u, float scalar)
{
  return Vector3(u.x * scalar, u.y * scalar, u.z * scalar);
}
inline Vector3 operator*(float scalar, Vector3 u)
{
  return Vector3(u.x * scalar, u.y * scalar, u.z * scalar);
}
inline Vector3 operator/(Vector3 u, float scalar)
{
  return Vector3(u.x / scalar, u.y / scalar, u.z / scalar);
}
 
// BtQuaternion
class BtQuaternion
{
public:
  BtQuaternion()
  {
    w = 1.0f;
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
  }
 
  BtQuaternion(float ww, float xx, float yy, float zz)
  {
    w = ww;
    x = xx;
    y = yy;
    z = zz;
  }
 
  BtQuaternion(float quat[4])
  {
    w = quat[0];
    x = quat[1];
    y = quat[2];
    z = quat[3];
  }
 
  BtQuaternion(double quat[4])
  {
    w = (float)quat[0];
    x = (float)quat[1];
    y = (float)quat[2];
    z = (float)quat[3];
  }
 
  static const BtQuaternion IDENTITY;
 
  //-----------------------------------------------------------------------
  inline bool operator== (const BtQuaternion& rhs) const
  {
    return (rhs.x == x) && (rhs.y == y) &&
      (rhs.z == z) && (rhs.w == w);
  }
  //-----------------------------------------------------------------------
  inline bool operator!= (const BtQuaternion& rhs) const
  {
    return !operator==(rhs);
  }
  //-----------------------------------------------------------------------
  BtQuaternion operator+ (const BtQuaternion& rkQ) const
  {
    return BtQuaternion(w + rkQ.w, x + rkQ.x, y + rkQ.y, z + rkQ.z);
  }
  //-----------------------------------------------------------------------
  BtQuaternion operator- (const BtQuaternion& rkQ) const
  {
    return BtQuaternion(w - rkQ.w, x - rkQ.x, y - rkQ.y, z - rkQ.z);
  }
  //-----------------------------------------------------------------------
  BtQuaternion operator-() const { return BtQuaternion(-w, -x, -y, -z); }
  //-----------------------------------------------------------------------
  BtQuaternion operator* (const BtQuaternion& rkQ) const
  {
    // NOTE:  Multiplication is not generally commutative, so in most
    // cases p*q != q*p.
 
    return BtQuaternion
    (
      w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
      w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
      w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
      w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
    );
  }
  //-----------------------------------------------------------------------
  BtQuaternion operator*(float s) const
  {
    return BtQuaternion(s * w, s * x, s * y, s * z);
  }
  //-----------------------------------------------------------------------
  friend BtQuaternion operator*(float s, const BtQuaternion& q)
  {
    return q * s;
  }
  //-----------------------------------------------------------------------
  BtQuaternion Inverse() const
  {
    float fNorm = w * w + x * x + y * y + z * z;
    if (fNorm > 0.0)
    {
      float fInvNorm = 1.0f / fNorm;
      return BtQuaternion(w*fInvNorm, -x * fInvNorm, -y * fInvNorm, -z * fInvNorm);
    }
    else
    {
      // return an invalid result to flag the error
      return BtQuaternion();
    }
  }
  //-----------------------------------------------------------------------
  Vector3 operator* (const Vector3& v) const
  {
    // nVidia SDK implementation
    Vector3 uv, uuv;
    Vector3 qvec(x, y, z);
    uv = qvec.CrossProduct(v);
    uuv = qvec.CrossProduct(uv);
    uv *= (2.0f * w);
    uuv *= 2.0f;
 
    return v + uv + uuv;
  }
  //-----------------------------------------------------------------------
  static BtQuaternion FromRotationMatrix(double rotMatrix[16], bool reverseX = false, bool reverseY = true)
  {
    // Origin is double numbers, target is float. Convert from double to float as late as possible, so do not write "f" suffix belong to floating numbers!
    BtQuaternion mQuat;
 
    //revert Y axis
    double kRot[3][3];
    kRot[0][0] = rotMatrix[0];
    kRot[0][1] = rotMatrix[4];
    kRot[0][2] = rotMatrix[8];
 
    if (reverseY)
    {
      kRot[1][0] = -rotMatrix[1];
      kRot[1][1] = -rotMatrix[5];
      kRot[1][2] = -rotMatrix[9];
    }
    else
    {
      kRot[1][0] = rotMatrix[1];
      kRot[1][1] = rotMatrix[5];
      kRot[1][2] = rotMatrix[9];
    }
 
    kRot[2][0] = rotMatrix[2];
    kRot[2][1] = rotMatrix[6];
    kRot[2][2] = rotMatrix[10];
 
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "BtQuaternion Calculus and Fast Animation".
    double fTrace = kRot[0][0] + kRot[1][1] + kRot[2][2];
    double fRoot;
 
    if (fTrace > 0.0)
    {
      // |w| > 1/2, may as well choose w > 1/2
      fRoot = sqrt(fTrace + 1.0);  // 2w
      mQuat.w = static_cast<float>(0.5*fRoot);
      fRoot = 0.5 / fRoot;  // 1/(4w)
      mQuat.x = static_cast<float>((kRot[2][1] - kRot[1][2])*fRoot);
      mQuat.y = static_cast<float>((kRot[0][2] - kRot[2][0])*fRoot);
      mQuat.z = static_cast<float>((kRot[1][0] - kRot[0][1])*fRoot);
    }
    else
    {
      // |w| <= 1/2
      static size_t s_iNext[3] = { 1, 2, 0 };
      size_t i = 0;
      if (kRot[1][1] > kRot[0][0])
        i = 1;
      if (kRot[2][2] > kRot[i][i])
        i = 2;
 
      size_t j = s_iNext[i];
      size_t k = s_iNext[j];
 
      fRoot = sqrt(kRot[i][i] - kRot[j][j] - kRot[k][k] + 1.0);
      float* apkQuat[3] = { &mQuat.x, &mQuat.y, &mQuat.z };
      *apkQuat[i] = static_cast<float>(0.5*fRoot);
      fRoot = 0.5 / fRoot;
      mQuat.w = static_cast<float>((kRot[k][j] - kRot[j][k])*fRoot);
      *apkQuat[j] = static_cast<float>((kRot[j][i] + kRot[i][j])*fRoot);
      *apkQuat[k] = static_cast<float>((kRot[k][i] + kRot[i][k])*fRoot);
    }
 
    if (reverseX)
    {
      mQuat.x = -mQuat.x;
      mQuat.w = -mQuat.w;
    }
 
    //normalize
    float len = mQuat.w*mQuat.w + mQuat.x*mQuat.x + mQuat.y*mQuat.y + mQuat.z*mQuat.z;
    float factor = 1.0f / sqrt(len);
    mQuat.w *= factor;
    mQuat.x *= factor;
    mQuat.y *= factor;
    mQuat.z *= factor;
 
    return mQuat;
  }
  //-----------------------------------------------------------------------
  float Dot(const BtQuaternion& rkQ) const
  {
    return w * rkQ.w + x * rkQ.x + y * rkQ.y + z * rkQ.z;
  }
  //-----------------------------------------------------------------------
  float Norm() const { return sqrt(w * w + x * x + y * y + z * z); }
  //-----------------------------------------------------------------------
  float normalise(void)
  {
    float len = Norm();
    *this = 1.0f / len * *this;
    return len;
  }
  //-----------------------------------------------------------------------
  static BtQuaternion Slerp(float fT, const BtQuaternion& rkP,
    const BtQuaternion& rkQ, bool shortestPath)
  {
    float fCos = rkP.Dot(rkQ);
    BtQuaternion rkT;
 
    // Do we need to invert rotation?
    if (fCos < 0.0f && shortestPath)
    {
      fCos = -fCos;
      rkT = -rkQ;
    }
    else
    {
      rkT = rkQ;
    }
 
    if (fabs(fCos) < 1 - 1e-03)
    {
      // Standard case (slerp)
      float fSin = sqrt(1 - (fCos * fCos));
      float fAngle = std::atan2(fSin, fCos);
      float fInvSin = 1.0f / fSin;
      float fCoeff0 = std::sin((1.0f - fT) * fAngle) * fInvSin;
      float fCoeff1 = std::sin(fT * fAngle) * fInvSin;
      return fCoeff0 * rkP + fCoeff1 * rkT;
    }
    else
    {
      // There are two situations:
      // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
      //    interpolation safely.
      // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
      //    are an infinite number of possibilities interpolation. but we haven't
      //    have method to fix this case, so just use linear interpolation here.
      BtQuaternion t = (1.0f - fT) * rkP + fT * rkT;
      // taking the complement requires renormalisation
      t.normalise();
      return t;
    }
  }
protected:
private:
 
public:
  float x;
  float y;
  float z;
  float w;
protected:
private:
};
 
 
 
class FilterDoubleExponential
{
private:
  std::vector<cv::Point2f> m_filteredPoints;
  std::vector<cv::Point2f> m_prevPoints;
  std::vector<cv::Point2f> m_prevfilteredPoints;
  std::vector<cv::Point2f> m_pointsTrend;
  float m_fSmoothing;
  float m_fCorrection;
  float m_fPrediction;
  float m_fJitterRadius;
  float m_fMaxDeviationRadius;
  int m_dwFrameCount;
 
public:
  FilterDoubleExponential() { Init(); }
  ~FilterDoubleExponential() { Shutdown(); }
 
  void Init(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
  {
    m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
    m_fSmoothing = fSmoothing;                   // How much smothing will occur.  Will lag when too high
    m_fCorrection = fCorrection;                 // How much to correct back from prediction.  Can make things springy
    m_fPrediction = fPrediction;                 // Amount of prediction into the future to use. Can over shoot when too high
    m_fJitterRadius = fJitterRadius;             // Size of the radius where jitter is removed. Can do too much smoothing when too high
 
    Reset();
  }
 
  void Shutdown()
  {
  }
 
  void Reset()
  {
    m_dwFrameCount = 0;
    m_filteredPoints.clear();
    m_prevPoints.clear();
    m_prevfilteredPoints.clear();
    m_pointsTrend.clear();
    for (unsigned int k = 0; k < 4; ++k)
    {
      m_filteredPoints.push_back(cv::Point2f(0.0f, 0.0f));
      m_prevPoints.push_back(cv::Point2f(0.0f, 0.0f));
      m_prevfilteredPoints.push_back(cv::Point2f(0.0f, 0.0f));
      m_pointsTrend.push_back(cv::Point2f(0.0f, 0.0f));
    }
  }
 
  void Update(std::vector<cv::Point2f> points)
  {
    cv::Point2f diffPoints;
    std::vector<cv::Point2f> trendPoints;
    float fDiff;
 
    for (unsigned int k = 0; k < 4; ++k)
    {
      trendPoints.push_back(cv::Point2f(0.0f, 0.0f));
    }
 
    // Initial start values
    if (m_dwFrameCount == 0)
    {
      for (unsigned int i = 0; i < 4; i++)
      {
        m_filteredPoints[i] = points[i];
      }
      m_dwFrameCount++;
    }
    else if (m_dwFrameCount == 1)
    {
      for (unsigned int i = 0; i < 4; i++)
      {
        m_filteredPoints[i] = points[i] + m_prevPoints[i];
        m_filteredPoints[i].x *= 0.5f;
        m_filteredPoints[i].y *= 0.5f;
 
        diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
        cv::Point2f cdiff = diffPoints;
        cdiff.x *= m_fCorrection;
        cdiff.y *= m_fCorrection;
 
        cv::Point2f pdiff = m_pointsTrend[i];
        pdiff.x *= 1.0f - m_fCorrection;
        pdiff.y *= 1.0f - m_fCorrection;
        trendPoints[i] = cdiff + pdiff;
      }
 
      m_dwFrameCount++;
    }
    else
    {
      for (unsigned int i = 0; i < 4; i++)
      {
        diffPoints = points[i] - m_prevfilteredPoints[i];
 
        fDiff = fabs(sqrt(diffPoints.x * diffPoints.x + diffPoints.y * diffPoints.y));
        if (fDiff <= m_fJitterRadius)
        {
          diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
          cv::Point2f cVal = points[i];
          cVal.x *= fDiff / m_fJitterRadius;
          cVal.y *= fDiff / m_fJitterRadius;
 
          cv::Point2f pVal = m_prevfilteredPoints[i];
          pVal.x *= 1.0f - fDiff / m_fJitterRadius;
          pVal.y *= 1.0f - fDiff / m_fJitterRadius;
          m_filteredPoints[i] = cVal + pVal;
        }
        else
        {
          m_filteredPoints[i] = points[i];
        }
 
        // Now the double exponential smoothing filter
        cv::Point2f cVal = m_filteredPoints[i];
        cVal.x *= 1.0f - m_fSmoothing;
        cVal.y *= 1.0f - m_fSmoothing;
 
        cv::Point2f pVal = m_prevfilteredPoints[i] + m_pointsTrend[i];
        pVal.x *= m_fSmoothing;
        pVal.y *= m_fSmoothing;
        m_filteredPoints[i] = cVal + pVal;
 
        diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
        cVal = diffPoints;
        cVal.x *= m_fCorrection;
        cVal.y *= m_fCorrection;
 
        pVal = m_pointsTrend[i];
        pVal.x *= 1.0f - m_fCorrection;
        pVal.y *= 1.0f - m_fCorrection;
 
        trendPoints[i] = cVal + pVal;
 
        // Predict into the future to reduce latency
        cVal = trendPoints[i];
        cVal.x *= m_fPrediction;
        cVal.y *= m_fPrediction;
 
        cv::Point2f predictedPoint = m_filteredPoints[i] + cVal;
 
        // Check that we are not too far away from raw data
        diffPoints = predictedPoint - points[i];
        fDiff = fabs(sqrt(diffPoints.x * diffPoints.x + diffPoints.y * diffPoints.y));
 
        if (fDiff > m_fMaxDeviationRadius)
        {
          cVal = predictedPoint;
          cVal.x *= m_fMaxDeviationRadius / fDiff;
          cVal.y *= m_fMaxDeviationRadius / fDiff;
 
          pVal = points[i];
          pVal.x *= 1.0f - m_fMaxDeviationRadius / fDiff;
          pVal.y *= 1.0f - m_fMaxDeviationRadius / fDiff;
 
          predictedPoint = cVal + pVal;
        }
 
        m_filteredPoints[i] = predictedPoint;
      }
    }
 
    for (unsigned int i = 0; i < 4; i++)
    {
      m_prevfilteredPoints[i] = m_filteredPoints[i];
      m_prevPoints[i] = points[i];
      m_pointsTrend[i] = trendPoints[i];
    }
  }
 
  std::vector<cv::Point2f> GetFilteredPoints()
  {
    return m_filteredPoints;
  }
};
 
 
class FilterDoubleExponentialDouble
{
private:
  std::vector<double> m_filteredPoints;
  std::vector<double> m_prevPoints;
  std::vector<double> m_prevfilteredPoints;
  std::vector<double> m_pointsTrend;
  float m_fSmoothing;
  float m_fCorrection;
  float m_fPrediction;
  float m_fJitterRadius;
  float m_fMaxDeviationRadius;
  int m_dwFrameCount;
 
public:
  FilterDoubleExponentialDouble() { Init(); }
  ~FilterDoubleExponentialDouble() { Shutdown(); }
 
  void Init(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
  {
    m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
    m_fSmoothing = fSmoothing;                   // How much smothing will occur.  Will lag when too high
    m_fCorrection = fCorrection;                 // How much to correct back from prediction.  Can make things springy
    m_fPrediction = fPrediction;                 // Amount of prediction into the future to use. Can over shoot when too high
    m_fJitterRadius = fJitterRadius;             // Size of the radius where jitter is removed. Can do too much smoothing when too high
 
    Reset();
  }
 
  void Shutdown()
  {
  }
 
  void Reset()
  {
    m_dwFrameCount = 0;
    m_filteredPoints.clear();
    m_prevPoints.clear();
    m_prevfilteredPoints.clear();
    m_pointsTrend.clear();
  }
 
  void Update(std::vector<double> points)
  {
    double diffPoints;
    std::vector<double> trendPoints;
    float fDiff;
 
    if (points.size() != m_filteredPoints.size())
    {
      m_filteredPoints = points;
      m_prevPoints = points;
      m_prevfilteredPoints = points;
      m_pointsTrend = points;
 
      /*for (unsigned int k = 0; k < points.size(); ++k)
      {
        m_filteredPoints.push_back(0.0);
        m_prevPoints.push_back(0.0);
        m_prevfilteredPoints.push_back(0.0);
        m_pointsTrend.push_back(0.0);
      }*/
    }
 
    for (unsigned int k = 0; k < points.size(); ++k)
    {
      trendPoints.push_back(0.0);
    }
 
    // Initial start values
    if (m_dwFrameCount == 0)
    {
      for (unsigned int i = 0; i < points.size(); i++)
      {
        m_filteredPoints[i] = points[i];
      }
      m_dwFrameCount++;
    }
    else if (m_dwFrameCount == 1)
    {
      for (unsigned int i = 0; i < points.size(); i++)
      {
        m_filteredPoints[i] = points[i] + m_prevPoints[i];
        m_filteredPoints[i] *= 0.5f;
 
        diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
        double cdiff = diffPoints;
        cdiff *= m_fCorrection;
 
        double pdiff = m_pointsTrend[i];
        pdiff *= 1.0f - m_fCorrection;
        trendPoints[i] = cdiff + pdiff;
      }
 
      m_dwFrameCount++;
    }
    else
    {
      for (unsigned int i = 0; i < points.size(); i++)
      {
        diffPoints = points[i] - m_prevfilteredPoints[i];
 
        fDiff = fabs(sqrt(diffPoints * diffPoints));
        if (fDiff <= m_fJitterRadius)
        {
          diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
          double cVal = points[i];
          cVal *= fDiff / m_fJitterRadius;
 
          double pVal = m_prevfilteredPoints[i];
          pVal *= 1.0f - fDiff / m_fJitterRadius;
          m_filteredPoints[i] = cVal + pVal;
        }
        else
        {
          m_filteredPoints[i] = points[i];
        }
 
        // Now the double exponential smoothing filter
        double cVal = m_filteredPoints[i];
        cVal *= 1.0f - m_fSmoothing;
 
        double pVal = m_prevfilteredPoints[i] + m_pointsTrend[i];
        pVal *= m_fSmoothing;
 
        m_filteredPoints[i] = cVal + pVal;
 
        diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
        cVal = diffPoints;
        cVal *= m_fCorrection;
 
        pVal = m_pointsTrend[i];
        pVal *= 1.0f - m_fCorrection;
 
        trendPoints[i] = cVal + pVal;
 
        // Predict into the future to reduce latency
        cVal = trendPoints[i];
        cVal *= m_fPrediction;
 
        double predictedPoint = m_filteredPoints[i] + cVal;
 
        // Check that we are not too far away from raw data
        diffPoints = predictedPoint - points[i];
        fDiff = fabs(sqrt(diffPoints * diffPoints));
 
        if (fDiff > m_fMaxDeviationRadius)
        {
          cVal = predictedPoint;
          cVal *= m_fMaxDeviationRadius / fDiff;
 
          pVal = points[i];
          pVal *= 1.0f - m_fMaxDeviationRadius / fDiff;
 
          predictedPoint = cVal + pVal;
        }
 
        m_filteredPoints[i] = predictedPoint;
      }
    }
 
    for (unsigned int i = 0; i < points.size(); i++)
    {
      m_prevfilteredPoints[i] = m_filteredPoints[i];
      m_prevPoints[i] = points[i];
      m_pointsTrend[i] = trendPoints[i];
    }
  }
 
  std::vector<double> GetFilteredPoints()
  {
    return m_filteredPoints;
  }
};
 
 
class FilterDoubleExponential3f
{
private:
  std::vector<cv::Point3f> m_filteredPoints;
  std::vector<cv::Point3f> m_prevPoints;
  std::vector<cv::Point3f> m_prevfilteredPoints;
  std::vector<cv::Point3f> m_pointsTrend;
  float m_fSmoothing;
  float m_fCorrection;
  float m_fPrediction;
  float m_fJitterRadius;
  float m_fMaxDeviationRadius;
  int m_dwFrameCount;
 
public:
  FilterDoubleExponential3f() { Init(); }
  ~FilterDoubleExponential3f() { Shutdown(); }
 
  void Init(float fSmoothing = 0.25f, float fCorrection = 0.25f, float fPrediction = 0.25f, float fJitterRadius = 0.03f, float fMaxDeviationRadius = 0.05f)
  {
    m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
    m_fSmoothing = fSmoothing;                   // How much smothing will occur.  Will lag when too high
    m_fCorrection = fCorrection;                 // How much to correct back from prediction.  Can make things springy
    m_fPrediction = fPrediction;                 // Amount of prediction into the future to use. Can over shoot when too high
    m_fJitterRadius = fJitterRadius;             // Size of the radius where jitter is removed. Can do too much smoothing when too high
 
    Reset();
  }
 
  void Shutdown()
  {
  }
 
  void Reset()
  {
    m_dwFrameCount = 0;
    m_filteredPoints.clear();
    m_prevPoints.clear();
    m_prevfilteredPoints.clear();
    m_pointsTrend.clear();
    for (unsigned int k = 0; k < 2; ++k)
    {
      m_filteredPoints.push_back(cv::Point3f(0.0f, 0.0f, 0.0f));
      m_prevPoints.push_back(cv::Point3f(0.0f, 0.0f, 0.0f));
      m_prevfilteredPoints.push_back(cv::Point3f(0.0f, 0.0f, 0.0f));
      m_pointsTrend.push_back(cv::Point3f(0.0f, 0.0f, 0.0f));
    }
  }
 
  void Update(std::vector<cv::Point3f> points)
  {
    cv::Point3f diffPoints;
    std::vector<cv::Point3f> trendPoints;
    float fDiff;
 
    for (unsigned int k = 0; k < 2; ++k)
    {
      trendPoints.push_back(cv::Point3f(0.0f, 0.0f, 0.0f));
    }
 
    // Initial start values
    if (m_dwFrameCount == 0)
    {
      for (unsigned int i = 0; i < 2; i++)
      {
        m_filteredPoints[i] = points[i];
      }
      m_dwFrameCount++;
    }
    else if (m_dwFrameCount == 1)
    {
      for (unsigned int i = 0; i < 2; i++)
      {
        m_filteredPoints[i] = points[i] + m_prevPoints[i];
        m_filteredPoints[i].x *= 0.5f;
        m_filteredPoints[i].y *= 0.5f;
        m_filteredPoints[i].z *= 0.5f;
 
        diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
        cv::Point3f cdiff = diffPoints;
        cdiff.x *= m_fCorrection;
        cdiff.y *= m_fCorrection;
        cdiff.z *= m_fCorrection;
 
        cv::Point3f pdiff = m_pointsTrend[i];
        pdiff.x *= 1.0f - m_fCorrection;
        pdiff.y *= 1.0f - m_fCorrection;
        pdiff.z *= 1.0f - m_fCorrection;
        trendPoints[i] = cdiff + pdiff;
      }
 
      m_dwFrameCount++;
    }
    else
    {
      for (unsigned int i = 0; i < 2; i++)
      {
        diffPoints = points[i] - m_prevfilteredPoints[i];
 
        fDiff = fabs(sqrt(diffPoints.x * diffPoints.x + diffPoints.y * diffPoints.y));
        if (fDiff <= m_fJitterRadius)
        {
          diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
          cv::Point3f cVal = points[i];
          cVal.x *= fDiff / m_fJitterRadius;
          cVal.y *= fDiff / m_fJitterRadius;
          cVal.z *= fDiff / m_fJitterRadius;
 
          cv::Point3f pVal = m_prevfilteredPoints[i];
          pVal.x *= 1.0f - fDiff / m_fJitterRadius;
          pVal.y *= 1.0f - fDiff / m_fJitterRadius;
          pVal.z *= 1.0f - fDiff / m_fJitterRadius;
          m_filteredPoints[i] = cVal + pVal;
        }
        else
        {
          m_filteredPoints[i] = points[i];
        }
 
        // Now the double exponential smoothing filter
        cv::Point3f cVal = m_filteredPoints[i];
        cVal.x *= 1.0f - m_fSmoothing;
        cVal.y *= 1.0f - m_fSmoothing;
        cVal.z *= 1.0f - m_fSmoothing;
 
        cv::Point3f pVal = m_prevfilteredPoints[i] + m_pointsTrend[i];
        pVal.x *= m_fSmoothing;
        pVal.y *= m_fSmoothing;
        pVal.z *= m_fSmoothing;
        m_filteredPoints[i] = cVal + pVal;
 
        diffPoints = m_filteredPoints[i] - m_prevfilteredPoints[i];
 
        cVal = diffPoints;
        cVal.x *= m_fCorrection;
        cVal.y *= m_fCorrection;
        cVal.z *= m_fCorrection;
 
        pVal = m_pointsTrend[i];
        pVal.x *= 1.0f - m_fCorrection;
        pVal.y *= 1.0f - m_fCorrection;
        pVal.z *= 1.0f - m_fCorrection;
 
        trendPoints[i] = cVal + pVal;
 
        // Predict into the future to reduce latency
        cVal = trendPoints[i];
        cVal.x *= m_fPrediction;
        cVal.y *= m_fPrediction;
        cVal.z *= m_fPrediction;
 
        cv::Point3f predictedPoint = m_filteredPoints[i] + cVal;
 
        // Check that we are not too far away from raw data
        diffPoints = predictedPoint - points[i];
        fDiff = fabs(sqrt(diffPoints.x * diffPoints.x + diffPoints.y * diffPoints.y));
 
        if (fDiff > m_fMaxDeviationRadius)
        {
          cVal = predictedPoint;
          cVal.x *= m_fMaxDeviationRadius / fDiff;
          cVal.y *= m_fMaxDeviationRadius / fDiff;
          cVal.z *= m_fMaxDeviationRadius / fDiff;
 
          pVal = points[i];
          pVal.x *= 1.0f - m_fMaxDeviationRadius / fDiff;
          pVal.y *= 1.0f - m_fMaxDeviationRadius / fDiff;
          pVal.z *= 1.0f - m_fMaxDeviationRadius / fDiff;
 
          predictedPoint = cVal + pVal;
        }
 
        m_filteredPoints[i] = predictedPoint;
      }
    }
 
    for (unsigned int i = 0; i < 2; i++)
    {
      m_prevfilteredPoints[i] = m_filteredPoints[i];
      m_prevPoints[i] = points[i];
      m_pointsTrend[i] = trendPoints[i];
    }
  }
 
  std::vector<cv::Point3f> GetFilteredPoints()
  {
    return m_filteredPoints;
  }
};
 
static bool NiceHomography(cv::Mat Homography)
{
  cv::Mat H;
  Homography.convertTo(H, CV_32FC1);
 
  const float det = H.at<float>(0, 0) * H.at<float>(1, 1) - H.at<float>(1, 0) * H.at<float>(0, 1);
  if (det < 0)
    return false;
 
  const float N1 = sqrt(H.at<float>(0, 0) * H.at<float>(0, 0) + H.at<float>(1, 0) * H.at<float>(1, 0));
  if (N1 > 4 || N1 < 0.1)
    return false;
 
  const float N2 = sqrt(H.at<float>(0, 1) * H.at<float>(0, 1) + H.at<float>(1, 1) * H.at<float>(1, 1));
  if (N2 > 4 || N2 < 0.1)
    return false;
 
  const float N3 = sqrt(H.at<float>(2, 0) * H.at<float>(2, 0) + H.at<float>(2, 1) * H.at<float>(2, 1));
  if (N3 > 0.002)
    return false;
 
  return true;
}
 
// Converts a given Rotation Matrix to Euler angles
static cv::Point3f rot2euler(const cv::Mat &rotationMatrix)
{
  cv::Point3d euler;
 
  double m00 = rotationMatrix.at<float>(0, 0);
  double m02 = rotationMatrix.at<float>(0, 2);
  double m10 = rotationMatrix.at<float>(1, 0);
  double m11 = rotationMatrix.at<float>(1, 1);
  double m12 = rotationMatrix.at<float>(1, 2);
  double m20 = rotationMatrix.at<float>(2, 0);
  double m22 = rotationMatrix.at<float>(2, 2);
 
  double x, y, z;
 
  // Assuming the angles are in radians.
  if (m10 > 0.998f) { // singularity at north pole
    x = 0;
    y = CV_PI / 2;
    z = atan2(m02, m22);
  }
  else if (m10 < -0.998f) { // singularity at south pole
    x = 0;
    y = -CV_PI / 2;
    z = atan2(m02, m22);
  }
  else
  {
    x = atan2(-m12, m11);
    y = asin(m10);
    z = atan2(-m20, m00);
  }
 
  euler.x = x;
  euler.y = y;
  euler.z = z;
 
  return euler;
}
 
// Converts a given Euler angles to Rotation Matrix
static cv::Mat euler2rot(const cv::Point3f &euler)
{
  cv::Mat rotationMatrix(3, 3, CV_32F);
 
  float x = euler.x;
  float y = euler.y;
  float z = euler.z;
 
  // Assuming the angles are in radians.
  float ch = cos(z);
  float sh = sin(z);
  float ca = cos(y);
  float sa = sin(y);
  float cb = cos(x);
  float sb = sin(x);
 
  float m00, m01, m02, m10, m11, m12, m20, m21, m22;
 
  m00 = ch * ca;
  m01 = sh * sb - ch * sa*cb;
  m02 = ch * sa*sb + sh * cb;
  m10 = sa;
  m11 = ca * cb;
  m12 = -ca * sb;
  m20 = -sh * ca;
  m21 = sh * sa*cb + ch * sb;
  m22 = -sh * sa*sb + ch * cb;
 
  rotationMatrix.at<float>(0, 0) = m00;
  rotationMatrix.at<float>(0, 1) = m01;
  rotationMatrix.at<float>(0, 2) = m02;
  rotationMatrix.at<float>(1, 0) = m10;
  rotationMatrix.at<float>(1, 1) = m11;
  rotationMatrix.at<float>(1, 2) = m12;
  rotationMatrix.at<float>(2, 0) = m20;
  rotationMatrix.at<float>(2, 1) = m21;
  rotationMatrix.at<float>(2, 2) = m22;
 
  return rotationMatrix;
}
 
#ifdef ANDROID
#include <android/log.h>
#define LOG_TAG "ScolApp"
#define LOGI(...) __android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__)
#define LOGW(...) __android_log_print(ANDROID_LOG_WARN, LOG_TAG, __VA_ARGS__)
#define LOGE(...) __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)
 
#include <sys/stat.h>
static std::string CopyFileFromAssetToSD(std::string source, std::string dest, std::string destdir)
{
  std::string out = "";
  char*(*GetUserPath)() = (char*(__cdecl *)()) SCgetExtra("GetUserPath");
 
  if (GetUserPath)
  {
    int n;
    FILE *r, *w;
    char buf[1024];
 
    std::string userPath = GetUserPath();
    LOGI("CopyFileFromAssetToSD > User path : %s", userPath.c_str());
 
    source = "apk/" + source;
 
    dest = userPath + "/" + destdir + "/" + dest;
    destdir = userPath + "/" + destdir;
 
    if (boost::filesystem::exists(dest))
    {
      LOGI("CopyFileFromAssetToSD > source already exist : %s", dest.c_str());
      out = dest;
      return out;
    }
 
    LOGI("CopyFileFromAssetToSD > make dir : %s", destdir.c_str());
    boost::filesystem::create_directories(destdir.c_str());
 
    r = fopen(source.c_str(), "rb");
    if (r == NULL)
    {
      LOGI("CopyFileFromAssetToSD > source not found : %s", source.c_str());
      return out;
    }
 
    w = fopen(dest.c_str(), "wb");
    if (w)
    {
      do
      {
        n = fread(buf, 1, 1024, r);
        if (n > 0) fwrite(buf, 1, n, w);
      } while (n == 1024);
      fclose(w);
    }
    else
    {
      fclose(r);
      LOGI("CopyFileFromAssetToSD > dest failed to create : %s", dest.c_str());
      return out;
    }
 
    fclose(r);
    LOGI("CopyFileFromAssetToSD > succeed : %s", dest.c_str());
    out = dest;
    return out;
  }
 
  return out;
}
 
static std::string cloneFileFromAssetToSD(std::string source)
{
  if (source.compare(0, 4, "APK/") == 0)
    source = source.substr(4, source.length());
 
  boost::filesystem::path file(source);
  return CopyFileFromAssetToSD(source, file.filename().generic_string(), file.parent_path().generic_string());
  }
#endif
 
static cv::Mat createMask(cv::Size img_size, std::vector<cv::Point2f>& pts)
{
  cv::Mat mask(img_size, CV_8UC1);
  mask = 0;
 
  // ax+by+c=0
  float a[4];
  float b[4];
  float c[4];
 
  a[0] = pts[3].y - pts[0].y;
  a[1] = pts[2].y - pts[1].y;
  a[2] = pts[1].y - pts[0].y;
  a[3] = pts[2].y - pts[3].y;
 
  b[0] = pts[0].x - pts[3].x;
  b[1] = pts[1].x - pts[2].x;
  b[2] = pts[0].x - pts[1].x;
  b[3] = pts[3].x - pts[2].x;
 
  c[0] = pts[0].y * pts[3].x - pts[3].y * pts[0].x;
  c[1] = pts[1].y * pts[2].x - pts[2].y * pts[1].x;
  c[2] = pts[0].y * pts[1].x - pts[1].y * pts[0].x;
  c[3] = pts[3].y * pts[2].x - pts[2].y * pts[3].x;
 
  float max_x, min_x, max_y, min_y;
  max_x = 0;
  min_x = img_size.width;
  max_y = 0;
  min_y = img_size.height;
 
  int i;
  for (i = 0; i < 4; i++)
  {
    if (pts[i].x > max_x)
      max_x = pts[i].x;
    if (pts[i].x < min_x)
      min_x = pts[i].x;
    if (pts[i].y > max_y)
      max_y = pts[i].y;
    if (pts[i].y < min_y)
      min_y = pts[i].y;
  }
 
  if (max_x >= img_size.width)
    max_x = img_size.width - 1;
  if (max_y >= img_size.height)
    max_y = img_size.height - 1;
  if (min_x < 0)
    min_x = 0;
  if (min_y < 0)
    min_y = 0;
 
  unsigned char *ptr = mask.data;
  int x, y;
  int offset;
  float val[4];
  for (y = min_y; y <= max_y; y++)
  {
    offset = y * img_size.width;
    for (x = min_x; x <= max_x; x++)
    {
      for (i = 0; i < 4; i++)
      {
        val[i] = a[i] * x + b[i] * y + c[i];
      }
 
      if (val[0] * val[1] <= 0 && val[2] * val[3] <= 0)
        *(ptr + offset + x) = 255;
    }
  }
 
  return mask;
}
 
static std::vector<cv::Point2f> scalePoints(std::vector<cv::Point2f>& point_vec, double scale)
{
  std::vector<cv::Point2f> ret_vec;
 
  std::vector<cv::Point2f>::iterator itr = point_vec.begin();
  while (itr != point_vec.end())
  {
    ret_vec.push_back(*itr * scale);
    itr++;
  }
  return ret_vec;
}
 
// Check the validity of transformed rectangle shape
// the sign of outer products of each edge vector must be same
static bool checkRectShape(std::vector<cv::Point2f>& rectPts)
{
  if (rectPts.size() != 4)
    return false;
 
  bool result = true;
  float vec[4][2];
  unsigned int i, j;
 
  vec[0][0] = rectPts[1].x - rectPts[0].x;
  vec[0][1] = rectPts[1].y - rectPts[0].y;
  vec[1][0] = rectPts[2].x - rectPts[1].x;
  vec[1][1] = rectPts[2].y - rectPts[1].y;
  vec[2][0] = rectPts[3].x - rectPts[2].x;
  vec[2][1] = rectPts[3].y - rectPts[2].y;
  vec[3][0] = rectPts[0].x - rectPts[3].x;
  vec[3][1] = rectPts[0].y - rectPts[3].y;
 
  int s;
  float val = vec[3][0] * vec[0][1] - vec[3][1] * vec[0][0];
  if (val > 0)
    s = 1;
  else
    s = -1;
 
  for (i = 0; i < 3; i++)
  {
    val = vec[i][0] * vec[i + 1][1] - vec[i][1] * vec[i + 1][0];
    if (val * s <= 0) {
      result = false;
      break;
    }
  }
 
  if (cv::isContourConvex(cv::Mat(rectPts)))
  {
    for (j = 0; j < rectPts.size(); j++)
    {
      int pdx = j == 0 ? rectPts.size() - 1 : j - 1; // predecessor of idx
      int sdx = j == rectPts.size() - 1 ? 0 : j + 1; // successor of idx
 
      cv::Point v1 = rectPts[sdx] - rectPts[j];
      cv::Point v2 = rectPts[pdx] - rectPts[j];
 
      // one of the low interior angle + within upper 90% of region the marker seems bad
      double angle = acos(static_cast<double>(v1.x*v2.x + v1.y*v2.y) / (cv::norm(v1) * cv::norm(v2)));
      if (angle < 0.9)
      {
        result = false;
        break;
      }
    }
  }
 
  return result;
}
 
static void rotateImage(const cv::Mat &input, cv::Mat &output, cv::Mat &outwarp, double alpha, double beta, double gamma, double dx, double dy, double dz, double f)
{
  alpha = alpha * (CV_PI / 180.);
  beta = beta * (CV_PI / 180.);
  gamma = gamma * (CV_PI / 180.);
 
  // get width and height for ease of use in matrices
  double w = (double)input.cols;
  double h = (double)input.rows;
 
  // Projection 2D -> 3D matrix
  cv::Mat A1 = (cv::Mat_<double>(4, 3) <<
    1, 0, -w / 2,
    0, 1, -h / 2,
    0, 0, 0,
    0, 0, 1);
 
  // Rotation matrices around the X, Y, and Z axis
  cv::Mat RX = (cv::Mat_<double>(4, 4) <<
    1, 0, 0, 0,
    0, cos(alpha), -sin(alpha), 0,
    0, sin(alpha), cos(alpha), 0,
    0, 0, 0, 1);
 
  cv::Mat RY = (cv::Mat_<double>(4, 4) <<
    cos(beta), 0, -sin(beta), 0,
    0, 1, 0, 0,
    sin(beta), 0, cos(beta), 0,
    0, 0, 0, 1);
 
  cv::Mat RZ = (cv::Mat_<double>(4, 4) <<
    cos(gamma), -sin(gamma), 0, 0,
    sin(gamma), cos(gamma), 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1);
 
  // Composed rotation matrix with (RX, RY, RZ)
  cv::Mat R = RX * RY * RZ;
 
  // Translation matrix
  cv::Mat T = (cv::Mat_<double>(4, 4) <<
    1, 0, 0, dx,
    0, 1, 0, dy,
    0, 0, 1, dz,
    0, 0, 0, 1);
  // 3D -> 2D matrix
  cv::Mat A2 = (cv::Mat_<double>(3, 4) <<
    f, 0, w / 2, 0,
    0, f, h / 2, 0,
    0, 0, 1, 0);
 
  // Final transformation matrix
  outwarp = A2 * (T * (R * A1));
 
  // Apply matrix transformation
  warpPerspective(input, output, outwarp, input.size(), cv::INTER_LANCZOS4);
}
 
#endif