sSlam_orb.cpp

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
 
-----------------------------------------------------------------------------
*/
 
//#include "sService.h"
 
#include "ArManager.h"
#include "ArCameraParam.h"
#include "sSlam_orb.h"
 
#include "ORB_SLAM2/Random.h"
 
#include <vector>
 
#ifdef WIN32
#include <direct.h>
#define GetCurrentDir _getcwd
#else
#include <unistd.h>
#define GetCurrentDir getcwd
#endif
 
std::string GetCurrentWorkingDir(void)
{
  char buff[FILENAME_MAX];
  GetCurrentDir(buff, FILENAME_MAX);
  std::string current_working_dir(buff);
 
  for (unsigned int i = 0; i < current_working_dir.length(); i++)
  {
    if (current_working_dir.substr(i, 1) == "\\")
      current_working_dir.replace(i, 1, "/");
  }
  return current_working_dir;
}
 
const float eps = 1e-4;
cv::Mat ExpSO3(const float &x, const float &y, const float &z)
{
  cv::Mat I = cv::Mat::eye(3, 3, CV_32F);
  const float d2 = x * x + y * y + z * z;
  const float d = sqrt(d2);
  cv::Mat W;
  W = (cv::Mat_<float>(3, 3) << 0, -z, y, z, 0, -x, -y, x, 0);
 
  if (d < eps)
    return (I + W + 0.5f*W*W);
  else
    return (I + W * sin(d) / d + W * W*(1.0f - cos(d)) / d2);
}
 
cv::Mat ExpSO3(const cv::Mat &v)
{
  return ExpSO3(v.at<float>(0), v.at<float>(1), v.at<float>(2));
}
 
cv::Point2f Camera2Pixel(cv::Mat poseCamera, cv::Mat mk)
{
  return cv::Point2f(
    poseCamera.at<float>(0, 0) / poseCamera.at<float>(2, 0)*mk.at<float>(0, 0) + mk.at<float>(0, 2),
    poseCamera.at<float>(1, 0) / poseCamera.at<float>(2, 0)*mk.at<float>(1, 1) + mk.at<float>(1, 2)
  );
}
 
cv::Mat Plane::DetectPlane(const cv::Mat Tcw, const std::vector<ORB_SLAM2::MapPoint*> &vMPs, const int iterations)
{
  // Retrieve 3D points
  std::vector<cv::Mat> vPoints;
  vPoints.reserve(vMPs.size());
  std::vector<ORB_SLAM2::MapPoint*> vPointMP;
  vPointMP.reserve(vMPs.size());
 
  for (size_t i = 0; i < vMPs.size(); i++)
  {
    ORB_SLAM2::MapPoint* pMP = vMPs[i];
    if (pMP)
    {
      if (pMP->Observations() > 5)
      {
        vPoints.push_back(pMP->GetWorldPos());
        vPointMP.push_back(pMP);
      }
    }
  }
 
  const int N = vPoints.size();
 
  if (N < 50)
    return mTpw;
 
  // Indices for minimum set selection
  std::vector<size_t> vAllIndices;
  vAllIndices.reserve(N);
  std::vector<size_t> vAvailableIndices;
 
  for (int i = 0; i < N; i++)
  {
    vAllIndices.push_back(i);
  }
 
  float bestDist = 1e10;
  std::vector<float> bestvDist;
 
  //RANSAC
  for (int n = 0; n < iterations; n++)
  {
    vAvailableIndices = vAllIndices;
 
    cv::Mat A(3, 4, CV_32F);
    A.col(3) = cv::Mat::ones(3, 1, CV_32F);
 
    // Get min set of points
    for (short i = 0; i < 3; ++i)
    {
      int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1);
 
      int idx = vAvailableIndices[randi];
 
      A.row(i).colRange(0, 3) = vPoints[idx].t();
 
      vAvailableIndices[randi] = vAvailableIndices.back();
      vAvailableIndices.pop_back();
    }
 
    cv::Mat u, w, vt;
    cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
 
    const float a = vt.at<float>(3, 0);
    const float b = vt.at<float>(3, 1);
    const float c = vt.at<float>(3, 2);
    const float d = vt.at<float>(3, 3);
 
    std::vector<float> vDistances(N, 0);
 
    const float f = 1.0f / sqrt(a*a + b * b + c * c + d * d);
 
    for (int i = 0; i < N; i++)
    {
      vDistances[i] = fabs(vPoints[i].at<float>(0)*a + vPoints[i].at<float>(1)*b + vPoints[i].at<float>(2)*c + d)*f;
    }
 
    std::vector<float> vSorted = vDistances;
    std::sort(vSorted.begin(), vSorted.end());
 
    int nth = std::max((int)(0.2*N), 20);
    const float medianDist = vSorted[nth];
 
    if (medianDist < bestDist)
    {
      bestDist = medianDist;
      bestvDist = vDistances;
    }
  }
 
  // Compute threshold inlier/outlier
  const float th = 1.4 * bestDist;
  std::vector<bool> vbInliers(N, false);
  int nInliers = 0;
  for (int i = 0; i < N; i++)
  {
    if (bestvDist[i] < th)
    {
      nInliers++;
      vbInliers[i] = true;
    }
  }
 
  std::vector<ORB_SLAM2::MapPoint*> vInlierMPs(nInliers, NULL);
  int nin = 0;
  for (int i = 0; i < N; i++)
  {
    if (vbInliers[i])
    {
      vInlierMPs[nin] = vPointMP[i];
      nin++;
    }
  }
 
  mMvMPs = vInlierMPs;
  mMTcw = Tcw.clone();
  mRang = -3.14f / 2 + ((float)rand() / RAND_MAX)*3.14f;
  Recompute();
  return mTpw;
}
 
void Plane::Recompute()
{
  const int N = mMvMPs.size();
 
  // Recompute plane with all points
  cv::Mat A = cv::Mat(N, 4, CV_32F);
  A.col(3) = cv::Mat::ones(N, 1, CV_32F);
 
  mOrigin = cv::Mat::zeros(3, 1, CV_32F);
 
  int nPoints = 0;
  for (int i = 0; i < N; i++)
  {
    ORB_SLAM2::MapPoint* pMP = mMvMPs[i];
    if (!pMP->isBad())
    {
      cv::Mat Xw = pMP->GetWorldPos();
      mOrigin += Xw;
      A.row(nPoints).colRange(0, 3) = Xw.t();
      nPoints++;
    }
  }
  A.resize(nPoints);
 
  cv::Mat u, w, vt;
  cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
 
  float a = vt.at<float>(3, 0);
  float b = vt.at<float>(3, 1);
  float c = vt.at<float>(3, 2);
 
  mOrigin = mOrigin * (1.0f / nPoints);
  const float f = 1.0f / sqrt(a * a + b * b + c * c);
 
  // Compute XC just the first time
  if (mXC.empty())
  {
    cv::Mat Oc = -mMTcw.colRange(0, 3).rowRange(0, 3).t() * mMTcw.rowRange(0, 3).col(3);
    mXC = Oc - mOrigin;
  }
 
  if ((mXC.at<float>(0) * a + mXC.at<float>(1) * b + mXC.at<float>(2) * c) < 0)
  {
    a = -a;
    b = -b;
    c = -c;
  }
 
  const float nx = a * f;
  const float ny = b * f;
  const float nz = c * f;
 
  mNormal = (cv::Mat_<float>(3, 1) << nx, ny, nz);
 
  cv::Mat up;
  up = (cv::Mat_<float>(3, 1) << 0.0f, 1.0f, 0.0f);
 
  cv::Mat v = up.cross(mNormal);
  const float sa = cv::norm(v);
  const float ca = up.dot(mNormal);
  const float ang = atan2(sa, ca);
  mTpw = cv::Mat::eye(4, 4, CV_32F);
 
  //mTpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * ang / sa) * ExpSO3(up * mRang);
  ExpSO3(v*ang / sa).copyTo(mTpw.rowRange(0, 3).colRange(0, 3));
  mOrigin.copyTo(mTpw.col(3).rowRange(0, 3));
}
 
Sslam::Sslam() : btopenutils::Thread()
{
  mIsDirty = false;
  needUpdate = false;
  mScale = 1.0f;
  mSlam = 0;
  mTimestamp = 0.0;
  mFound = false;
  mInitialized = false;
  mPlane2World = cv::Mat::eye(4, 4, CV_32F);
  mFullInit = 0;
 
  mParams.Camera.width = 640;
  mParams.Camera.height = 480;
  mParams.Camera.fx = 640.0;
  mParams.Camera.fy = 640.0;
  mParams.Camera.cx = 320.0;
  mParams.Camera.cy = 240.0;
  mParams.Camera.k1 = 0.0;
  mParams.Camera.k2 = 0.0;
  mParams.Camera.p1 = 0.0;
  mParams.Camera.p2 = 0.0;
  mParams.Camera.k3 = 0.0;
  mParams.Camera.bf = 0.0;
  mParams.Camera.fps = 30.0;
  mParams.Camera.RGB = 1;
 
  mParams.ORBextractor.nFeatures = 800;
  mParams.ORBextractor.nLevels = 8;
  mParams.ORBextractor.scaleFactor = 1.2;
  mParams.ORBextractor.iniThFAST = 20;
  mParams.ORBextractor.minThFAST = 7;
 
  mParams.DepthMapFactor = 1.0;
  mParams.ThDepth = 1;
 
  std::string strVocFile;
#ifdef ANDROID
  strVocFile = CopyFileFromAssetToSD("btdata/orbvoc.bin", "orbvoc.bin", "btdata");
#else
  strVocFile = GetCurrentWorkingDir() + "/plugins/btdata/orbvoc.bin";
#endif
 
  mVocabulary = new ORB_SLAM2::ORBVocabulary();
  mVocabulary->loadFromBinaryFile(strVocFile);
 
  // Run thread
  StartThreading(boost::bind(&Sslam::GoThread, this));
}
 
Sslam::~Sslam()
{
  StopThreading();
  if (mSlam)
    mSlam->Shutdown();
 
  SAFE_DELETE(mSlam);
  SAFE_DELETE(mVocabulary);
}
 
void Sslam::SetDirty()
{
  mIsDirty = true;
}
 
void Sslam::BuildCameraParam(const aruco::CameraParameters &camparam)
{
  mScale = 1.0f;
 
  int maxsize = std::max(camparam.CamSize.width, camparam.CamSize.height);
  if (maxsize > 640)
    mScale = 640.0f / (float)maxsize;
 
  /*if (maxsize > 1600)
    mScale = 0.4f;
  else if (maxsize > 1200)
    mScale = 0.53333f;
  else if (maxsize > 750)
    mScale = 0.85333f;
  else if (maxsize > 640)
    mScale = 0.9f;
  else
    mScale = 1.0f;
 
#if defined(ANDROID) || defined(APPLE_IOS) || defined(RPI)
  if (maxsize > 640)
    mScale *= 0.75f;
#endif
  */
 
  /*
  752.277145
  752.277145
  403.000000
  301.500000
  */
 
  //construct camParam
  mParams.Camera.width = (int)((float)camparam.CamSize.width * mScale);
  mParams.Camera.height = (int)((float)camparam.CamSize.height * mScale);
 
  mParams.Camera.cx = static_cast<float>(mParams.Camera.width) * 0.5f;
  mParams.Camera.cy = static_cast<float>(mParams.Camera.height) * 0.5f;
 
  float f = std::max(mParams.Camera.width, mParams.Camera.height);
  mParams.Camera.fx = f;
  mParams.Camera.fy = f;
 
  int wlvl = mParams.Camera.width;
  int hlvl = mParams.Camera.height;
  int pyrLevels = 1;
  while (wlvl % 2 == 0 && hlvl % 2 == 0 && wlvl*hlvl > 5000 && pyrLevels < 12)
  {
    wlvl /= 2;
    hlvl /= 2;
    pyrLevels++;
  }
 
  mParams.ORBextractor.nLevels = pyrLevels;
}
 
void Sslam::InitDetector()
{
  boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
 
  if (mSlam)
    mSlam->Shutdown();
 
  SAFE_DELETE(mSlam);
 
  try
  {
    // build a SLAM system
    mSlam = new ORB_SLAM2::System(mVocabulary, mParams, ORB_SLAM2::System::MONOCULAR, false);
    mSlam->DeactivateLocalizationMode();
    //mSlam->ActivateLocalizationMode();
  }
  catch (std::exception &)
  {
    SAFE_DELETE(mSlam);
  }
  mFound = false;
  mInitialized = false;
  mTimestamp = 0.0;
  ld.unlock();
}
 
Vector3 Sslam::GetCameraPosition()
{
  return mLastCamPos;
}
 
BtQuaternion Sslam::GetCameraOrientation()
{
  return mLastCamQuat;
}
 
bool Sslam::IsFound()
{
  return mFound;
}
 
void Sslam::Reset()
{
  InitDetector();
}
 
void Sslam::DrawLandmarks(cv::Mat image)
{
  if (mSlam && !image.empty())
  {
    //boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
    std::vector<cv::KeyPoint> keypts = mSlam->GetTrackedKeyPointsUn();
    cv::Scalar color(0, 255, 0);
 
    for (unsigned int i = 0; i < keypts.size(); i++)
    {
      cv::KeyPoint kp = keypts[i];
      cv::circle(image, kp.pt / mScale, 2, color);
    }
 
    if (!mCentroid.empty() && (mSlam->GetTrackingState() == 2))
    {
      ORB_SLAM2::Tracking* tracker = mSlam->GetTracker();
 
      std::vector<cv::Mat> axisPoints(4);
      axisPoints[0] = (cv::Mat_ <float>(4, 1) << 0.0, 0, 0, 1);
      axisPoints[1] = (cv::Mat_ <float>(4, 1) << 0.3, 0, 0, 1);
      axisPoints[2] = (cv::Mat_ <float>(4, 1) << 0.0, 0.3, 0, 1);
      axisPoints[3] = (cv::Mat_ <float>(4, 1) << 0, 0, 0.3, 1);
      std::vector<cv::Point2f> drawPoints(4);
      for (int i = 0; i < 4; i++)
      {
        axisPoints[i] = mPlane2Camera * axisPoints[i];
        drawPoints[i] = Camera2Pixel(axisPoints[i], tracker->GetCalibrationMatrix());
      }
 
      cv::line(image, drawPoints[0] / mScale, drawPoints[1] / mScale, cv::Scalar(250, 0, 0), 5);
      cv::line(image, drawPoints[0] / mScale, drawPoints[2] / mScale, cv::Scalar(0, 250, 0), 5);
      cv::line(image, drawPoints[0] / mScale, drawPoints[3] / mScale, cv::Scalar(0, 0, 250), 5);
 
      /*
      std::vector<cv::Point3f> drawRectPoints(8);
      drawRectPoints[0] = cv::Point3f(-0.15, 0, -0.15);
      drawRectPoints[1] = cv::Point3f(0.15, 0, -0.15);
      drawRectPoints[2] = cv::Point3f(-0.15, 0, 0.15);
      drawRectPoints[3] = cv::Point3f(-0.15, 0.3, -0.15);
      drawRectPoints[4] = cv::Point3f(-0.15, 0.3, 0.15);
      drawRectPoints[5] = cv::Point3f(0.15, 0.3, 0.15);
      drawRectPoints[6] = cv::Point3f(0.15, 0, 0.15);
      drawRectPoints[7] = cv::Point3f(0.15, 0.3, -0.15);
 
      cv::Mat Rcp, Tcp;
      std::vector<cv::Point2f> projectedPoints;
      cv::Rodrigues(mPlane2Camera.rowRange(0, 3).colRange(0, 3), Rcp);
      Tcp = mPlane2Camera.col(3).rowRange(0, 3);
      cv::projectPoints(drawRectPoints, Rcp, Tcp, tracker->GetCalibrationMatrix(), tracker->GetDistortionMatrix(), projectedPoints);
 
      cv::line(image, projectedPoints[0] / mScale, projectedPoints[1] / mScale, cv::Scalar(250, 0, 0), 5);
      cv::line(image, projectedPoints[0] / mScale, projectedPoints[2] / mScale, cv::Scalar(0, 250, 0), 5);
      cv::line(image, projectedPoints[0] / mScale, projectedPoints[3] / mScale, cv::Scalar(0, 0, 250), 5);
      cv::line(image, projectedPoints[1] / mScale, projectedPoints[7] / mScale, cv::Scalar(10, 0, 50), 2);
      cv::line(image, projectedPoints[3] / mScale, projectedPoints[7] / mScale, cv::Scalar(20, 0, 50), 2);
      cv::line(image, projectedPoints[3] / mScale, projectedPoints[4] / mScale, cv::Scalar(30, 0, 50), 2);
      cv::line(image, projectedPoints[2] / mScale, projectedPoints[4] / mScale, cv::Scalar(40, 0, 50), 2);
      cv::line(image, projectedPoints[1] / mScale, projectedPoints[6] / mScale, cv::Scalar(50, 0, 50), 2);
      cv::line(image, projectedPoints[2] / mScale, projectedPoints[6] / mScale, cv::Scalar(60, 0, 50), 2);
      cv::line(image, projectedPoints[4] / mScale, projectedPoints[5] / mScale, cv::Scalar(70, 0, 50), 2);
      cv::line(image, projectedPoints[5] / mScale, projectedPoints[6] / mScale, cv::Scalar(80, 0, 50), 2);
      cv::line(image, projectedPoints[5] / mScale, projectedPoints[7] / mScale, cv::Scalar(90, 0, 50), 2);
 
      cv::circle(image, projectedPoints[0] / mScale, 4, cv::Scalar(0, 0, 250), 1, 8);
      */
    }
 
    switch (mSlam->GetTrackingState())
    {
    case -1: {cv::putText(image, "SYSTEM NOT READY", cv::Point(20, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); }break;
    case 0: {cv::putText(image, "NO IMAGES YET", cv::Point(20, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); }break;
    case 1: {cv::putText(image, "SLAM NOT INITIALIZED", cv::Point(20, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); }break;
    case 2: {cv::putText(image, "SLAM ON", cv::Point(20, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1); break; }
    case 3: {cv::putText(image, "SLAM LOST", cv::Point(20, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1); break; }
    default:break;
    }
    //ld.unlock();
  }
}
 
void Sslam::Detect()
{
  int prevState = mSlam->GetTrackingState();
  cv::Mat pose;
  {
    boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
    pose = mSlam->TrackMonocular(mLastData.gray, mTimestamp);
    if (mSlam->MapChanged() || (mSlam->GetTrackingState() < 2))
    {
      mInitialized = false;
      mFound = false;
    }
 
    //mFound = (mSlam->GetTrackingState() == 2) ? true : false;
    ld.unlock();
  }
 
  // let the time to initialize completly
  if ((mFullInit < 10) && (mSlam->GetTrackingState() == 2))
    mFullInit += 1;
  else if (mSlam->GetTrackingState() != 2)
    mFullInit = 0;
 
  if (mFullInit < 10)
    mFound = false;
  else
  {
    if (pose.empty() || (mSlam->GetTrackingState() != 2))
    {
      mFound = false;
    }
    else
    {
      if (!mInitialized)
      {
        Plane mplane;
        const std::vector<ORB_SLAM2::MapPoint*> vpMPs = mSlam->GetTracker()->GetMap()->GetAllMapPoints();
        const std::vector<ORB_SLAM2::MapPoint*> vpTMPs = mSlam->GetTrackedMapPoints();
        if (vpMPs.size() > 150)
        {
          mPlane2World = mplane.DetectPlane(pose, vpTMPs, 50);
 
          if (!mPlane2World.empty())
          {
            mCentroid = mplane.mOrigin;
            mInitialized = true;
            mPlane2Camera = pose * mPlane2World;
          }
        }
      }
      else
      {
        mPlane2Camera = pose * mPlane2World;
      }
 
      if (!mInitialized || pose.empty())
      {
        mFound = false;
      }
      else
      {
        mFound = true;
        cv::Mat camtoplane = mPlane2Camera.inv();
 
        double modelview_matrix[16];
        modelview_matrix[0] = -camtoplane.at<float>(0, 0);
        modelview_matrix[1] = -camtoplane.at<float>(1, 0);
        modelview_matrix[2] = -camtoplane.at<float>(2, 0);
        modelview_matrix[3] = 0.0;
 
        modelview_matrix[4] = camtoplane.at<float>(0, 1);
        modelview_matrix[5] = camtoplane.at<float>(1, 1);
        modelview_matrix[6] = camtoplane.at<float>(2, 1);
        modelview_matrix[7] = 0.0;
 
        modelview_matrix[8]  = camtoplane.at<float>(0, 2);
        modelview_matrix[9]  = camtoplane.at<float>(1, 2);
        modelview_matrix[10] = camtoplane.at<float>(2, 2);
        modelview_matrix[11] = 0.0;
 
        modelview_matrix[12] = camtoplane.at<float>(0, 3);
        modelview_matrix[13] = camtoplane.at<float>(1, 3);
        modelview_matrix[14] = camtoplane.at<float>(2, 3);
        modelview_matrix[15] = 1.0;
 
        mLastCamQuat = BtQuaternion(0.0f, 0.0f, 1.0f, 0.0f) * BtQuaternion::FromRotationMatrix(modelview_matrix, mLastData.reversedBitmap);
        mLastCamPos = Vector3(static_cast<float>(mLastData.reversedBitmap ? -modelview_matrix[12] : modelview_matrix[12]), static_cast<float>(modelview_matrix[13]), static_cast<float>(modelview_matrix[14]));
      }
    }
  }
}
 
void Sslam::GoThread()
{
  std::chrono::steady_clock::time_point lastTimeStamp = std::chrono::steady_clock::now();
  while (IsRunning())
  {
    if (needUpdate)
    {
      ArManager* manager = ArManager::GetInstance();
      manager->GetLastData(mLastData);
 
      needUpdate = false;
 
      //cv::blur(mLastData.gray, mLastData.gray, cv::Size(2, 2));
 
      // Detect all markers seen on gray
 
      // camera size changed
      if (!mSlam || (((int)((float)mLastData.camParam.CamSize.width * mScale)) != mParams.Camera.width || ((int)((float)mLastData.camParam.CamSize.height * mScale)) != mParams.Camera.height))
        mIsDirty = true;
 
      // init detector
      if (mIsDirty)
      {
        mIsDirty = false;
        BuildCameraParam(mLastData.camParam);
        InitDetector();
        lastTimeStamp = std::chrono::steady_clock::now();
      }
 
      if (mScale != 1.0f)
        cv::resize(mLastData.gray, mLastData.gray, cv::Size((int)((float)mLastData.gray.cols * mScale), (int)((float)mLastData.gray.rows * mScale)), 0, 0, cv::INTER_LINEAR);
 
      //cv::equalizeHist(mLastData.gray, mLastData.gray);
 
      // thread to detect do it after mutex lock
      if (!mLastData.image.empty() && mSlam)
      {
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
        double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - lastTimeStamp).count();
        mTimestamp += ttrack;
        lastTimeStamp = std::chrono::steady_clock::now();
        Detect();
      }
    }
    else
    {
      //prevent cpu burn
      boost::this_thread::sleep_for(boost::chrono::milliseconds(1)); //DO not burn too much CPU
    }
  }
}