ArFaceMarker.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
 
-----------------------------------------------------------------------------
*/
 
/*
 Toolkit based on OpenCV library
 First version : dec 2010
 Author : Bastien BOURINEAU
*/
 
#include "ArFaceMarker.h"
#include <cstdio>
 
//#include "emotime/BoostEmoDetector.h"
//#include "emotime/SVMEmoDetector.h"
//#include "emotime/matrix_io.h"
//#include "emotime/TrainingParameters.h"
 
//#define DEBUG_FACE_TRACKING
 
ArFaceMarker::ArFaceMarker(unsigned int mid, float size) : ArFeaturedMarker(mid, size)
{
  m_bInitialized = false;
  m_nbInitFrames = 1;
  m_initFrames = 1;
  m_minInliners = 5;
  m_maxFeatures = 100;
 
  m_PosSmoothers.Init(0.25f, 0.25f, 0.25f, 0.1f, 0.2f);
 
  //debug
#ifdef DEBUG_FACE_TRACKING
  cv::namedWindow("Face Tracking", CV_WINDOW_AUTOSIZE);
  cv::namedWindow("Face Reference", CV_WINDOW_AUTOSIZE);
#endif
}
 
bool ArFaceMarker::IsInitialized()
{
  return m_bInitialized;
}
 
void ArFaceMarker::SetImage(const cv::Mat image, std::vector<cv::Rect> objRects)
{
  m_bInitialized = false;
 
  if (objRects.size() < 1)
    return;
 
  m_ObjRect = objRects[0];
  // reduce the rect to be sure to get features in the correct area
  float wdiff = (objRects[0].width / 3.0f);
  float hdiff = (objRects[0].height / 10.0f);
  float woff = objRects[0].width - (wdiff * 2.0f);
  float hoff = objRects[0].height - (hdiff * 8.0f);
  m_ObjRect.width = (int)wdiff * 2;
  m_ObjRect.height = (int)hdiff * 8;
  m_ObjRect.x = objRects[0].x + ((int)woff / 2);
  m_ObjRect.y = objRects[0].y + ((int)hoff / 2);
 
  image.copyTo(m_image);
  m_image = m_image(m_ObjRect);
  SetTrackedImage(m_image);
 
  int patchSize = m_image.cols / 6;
  if (!m_image.empty())
  {
    m_pcorners.clear();
    m_pcorners.push_back(cv::Point2f((float)m_ObjRect.x, (float)m_ObjRect.y));
    m_pcorners.push_back(cv::Point2f((float)m_ObjRect.x + m_ObjRect.width, (float)m_ObjRect.y));
    m_pcorners.push_back(cv::Point2f((float)m_ObjRect.x + m_ObjRect.width, (float)m_ObjRect.y + m_ObjRect.height));
    m_pcorners.push_back(cv::Point2f((float)m_ObjRect.x, (float)m_ObjRect.y + m_ObjRect.height));
 
    //init corners of the marker
    for (unsigned int i = 0; i < m_pcorners.size(); i++)
      this->push_back(cv::Point2f(m_pcorners[i].x, m_pcorners[i].y));
 
    cv::Mat dmask = cv::Mat(image.rows, image.cols, CV_8UC1);
    dmask.setTo(0);
    for (unsigned int i = 1; i < objRects.size(); i++)
    {
      cv::Rect orect = objRects[i];
      if (i > 0)
      {
        orect.x += objRects[0].x;
        orect.y += objRects[0].y;
      }
      cv::rectangle(dmask, orect, cv::Scalar(255), cv::FILLED);
    }
 
    cv::Size subPixWinSize(5, 5);
    cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.03f);
    try
    {
      m_ObjPoints.clear();
      cv::goodFeaturesToTrack(image, m_ObjPoints, m_maxFeatures, 0.01, 2, dmask, 3, false, 0.03f);
      
      //cornerSubPix(image, m_ObjPoints, subPixWinSize, cv::Size(-1, -1), termcrit);
 
      /*for (unsigned int i = 1; i < objRects.size(); i++)
      {
        cv::Point2f pt;
        pt.x = (float)(objRects[i].x + objRects[0].x + (patchSize / 2));
        pt.y = (float)(objRects[i].y + objRects[0].y + (objRects[i].height / 2));
        m_ObjPoints.push_back(pt);
 
        pt.x = (float)(objRects[i].x + objRects[0].x + objRects[i].width - (patchSize / 2));
        pt.y = (float)(objRects[i].y + objRects[0].y + (objRects[i].height / 2));
        m_ObjPoints.push_back(pt);
 
        pt.x = (float)(objRects[i].x + objRects[0].x + (objRects[i].width / 2));
        pt.y = (float)(objRects[i].y + objRects[0].y + (patchSize / 2));
        m_ObjPoints.push_back(pt);
 
        pt.x = (float)(objRects[i].x + objRects[0].x + (objRects[i].width / 2));
        pt.y = (float)(objRects[i].y + objRects[0].y + objRects[i].height - (patchSize / 2));
        m_ObjPoints.push_back(pt);
      }*/
 
      m_prevFramePoints = m_ObjPoints;
      m_lastFoundPoints.resize(m_ObjPoints.size(), true);
      m_lastFound = true;
 
#ifdef DEBUG_FACE_TRACKING
      cv::Mat mdebug;
      m_image.copyTo(mdebug);
      cv::cvtColor(mdebug, mdebug, CV_GRAY2BGR);
 
      for (int i = 0; i < (int)m_ObjPoints.size(); i++)
      {
        cv::circle(mdebug, m_ObjPoints[i] - cv::Point2f((float)m_ObjRect.x, (float)m_ObjRect.y), 2, cv::Scalar(0, 0, 255), 1);
      }
 
      int xoff = (objRects[0].width - m_ObjRect.width) / 2;
      int yoff = (objRects[0].height - m_ObjRect.height) / 2;
      for (unsigned int i = 1; i < objRects.size(); i++)
      {
        cv::Rect orect = objRects[i];
        orect.x -= xoff;
        orect.y -= yoff;
        cv::rectangle(mdebug, orect, cv::Scalar(255, 0, 0), 1);
      }
      cv::imshow("Face Reference", mdebug);
#endif
    }
    catch(std::exception& e)
    {
      MMechostr(MSKRUNTIME, "AR marker could not detect enough features points : %s", e.what());
      return;
    }
 
    if (m_ObjPoints.size() > m_minInliners)
      m_bInitialized = true;
  }
  m_bInitialized = true;
}
 
ArFaceMarker::~ArFaceMarker()
{
  vector<int> empty(2);
  empty[0] = 0; empty[1] = 0;  
  m_image = cv::Mat(empty);
  m_prevFrame = cv::Mat(empty);
  m_pcorners.clear();
 
#ifdef DEBUG_FACE_TRACKING
  cv::destroyWindow("Face Tracking");
  cv::destroyWindow("Face Reference");
#endif
}
 
bool ArFaceMarker::detectMotionFlow(cv::Mat &frame)
{
  std::vector<uchar> status;
  std::vector<float> err;
  cv::Size subPixWinSize(20, 20);
  cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 20, 0.03);
 
  if (!m_bInitialized)
    return false;
 
#ifdef DEBUG_FACE_TRACKING
    cv::Mat mdebug;
    frame.copyTo(mdebug);
    cv::cvtColor(mdebug, mdebug, CV_GRAY2BGR);
#endif
 
  cv::buildOpticalFlowPyramid(frame, m_nextPyr, subPixWinSize, 3, true);
 
  if (m_ObjPoints.empty() || m_prevPyr.empty())
  {
    m_prevPyr.swap(m_nextPyr);
    return false;
  }
 
  std::vector<cv::Point2f> nFramePoints;
  std::vector<cv::Point2f> nGoodFramePoints;
  std::vector<cv::Point2f> nGoodPoints;
  std::vector<cv::Mat> nGoodPatches;
  unsigned int goodtracks = 0;
  try
  {
    cv::calcOpticalFlowPyrLK(m_prevPyr, m_nextPyr, m_prevFramePoints, nFramePoints, status, err, subPixWinSize, 3, termcrit, /*cv::OPTFLOW_LK_GET_MIN_EIGENVALS|*/cv::OPTFLOW_FARNEBACK_GAUSSIAN, 0.001f);
    
    double movept = 0.0;
    unsigned int nbMatches = 0;
    for (unsigned int i = 0; i < nFramePoints.size(); ++i)
    {
      if (status[i] == 1)
      {
        movept += cv::norm(nFramePoints[i] - m_prevFramePoints[i]);
        nbMatches++;
      }
    }
 
    movept /= nbMatches;
    double threshold = 0.6;
    for (unsigned int i = 0; i < m_ObjPoints.size(); i++)
    {
      double dist = abs(cv::norm(nFramePoints[i] - m_prevFramePoints[i]));
 
      if (status[i] == 1 && err[i] <= 16.0 && (dist < 0.5 || dist < abs(movept * 2.0)))
      {
        goodtracks++;
        nGoodPoints.push_back(m_ObjPoints[i]);
        nGoodFramePoints.push_back(nFramePoints[i]);
        m_prevFramePoints[i] = nFramePoints[i];
        m_lastFoundPoints[i] = true;
 
#ifdef DEBUG_FACE_TRACKING
        cv::circle(mdebug, nFramePoints[i], 2, cv::Scalar(0, 255, 0), 1);
#endif
      }
      else
      {
        m_lastFoundPoints[i] = false;
        m_prevFramePoints[i] = m_prevFramePoints[i];
 
#ifdef DEBUG_FACE_TRACKING
          cv::circle(mdebug, m_prevFramePoints[i], 4, cv::Scalar(0, 0, 255), 2);
#endif
      }
    }
  }
  catch(std::exception &e)
  {
    std::string mess = e.what();
    m_bInitialized = false;
    m_prevPyr.clear();
    m_prevFramePoints.clear();
    m_initFrames = 0;
    return false;
  }
 
  m_prevPyr.swap(m_nextPyr);
  frame.copyTo(m_prevFrame);
 
  if (m_initFrames < m_nbInitFrames)
  {
    m_ObjPoints = nGoodPoints;
    m_prevFramePoints = nGoodFramePoints;
    m_initFrames ++;
  }
 
  if ((goodtracks < m_minInliners) || !GetCorners(nGoodPoints, nGoodFramePoints))
  {
    m_bInitialized = false;
    m_prevPyr.clear();
    m_prevFramePoints.clear();
    m_initFrames = 0;
    m_PosSmoothers.Reset();
    return false;
  }
  else
  {  
#ifdef DEBUG_FACE_TRACKING
    cv::Scalar color(255, 0, 0);
    int lineWidth = 1;
 
    cv::line(mdebug, this->at(0), this->at(1), color, lineWidth, CV_AA);
    cv::line(mdebug, this->at(1), this->at(2), color, lineWidth, CV_AA);
    cv::line(mdebug, this->at(2), this->at(3), color, lineWidth, CV_AA);
    cv::line(mdebug, this->at(3), this->at(0), color, lineWidth, CV_AA);
    cv::rectangle(mdebug, this->at(0)-cv::Point2f(2,2), this->at(0)+cv::Point2f(2,2), cv::Scalar(0,0,255), lineWidth, CV_AA);
    cv::rectangle(mdebug, this->at(1)-cv::Point2f(2,2), this->at(1)+cv::Point2f(2,2), cv::Scalar(0,255,0), lineWidth, CV_AA);
    cv::rectangle(mdebug, this->at(2)-cv::Point2f(2,2), this->at(2)+cv::Point2f(2,2), cv::Scalar(255,0,0), lineWidth, CV_AA);
 
    cv::Point2f mcenter;
    float mradius;
    cv::minEnclosingCircle(nGoodFramePoints, mcenter, mradius);
 
    cv::circle(mdebug, mcenter, (int)mradius, cv::Scalar(0,255,0));
    cv::imshow("Face Tracking", mdebug);
#endif
    return true;
  }
}
 
bool ArFaceMarker::GetCorners(std::vector<cv::Point2f> trainPoints, std::vector<cv::Point2f> framePoints)
{
  unsigned int i, j;
  std::vector<cv::Point2f> corners;
  cv::Mat transformedPoints;
  cv::Mat HMatrix;
  bool isvalid = false;
 
  if (framePoints.empty())
  {
    return false;
  }
 
  //set corners size
  corners.resize(4);
  unsigned int nbItt = 3;
  try
  {
    for (i = 0; (i < nbItt) && !isvalid; i++)
    {
      HMatrix = findHomography(cv::Mat(trainPoints), cv::Mat(framePoints), 0/*cv::RANSAC*/, 20.0);
    
      // transform points
      cv::perspectiveTransform(cv::Mat(m_pcorners), transformedPoints, HMatrix);
 
      for (j = 0; j < corners.size(); j++)
        corners[j] = transformedPoints.at<cv::Point2f>(j, 0);
 
      if ((corners.size() == 4) && (cv::isContourConvex(cv::Mat(corners))))
      {
        bool validCorners = true;
        for (j = 0; j < corners.size(); j++)
        {
          int pdx = j == 0 ? corners.size() - 1 : j - 1; // predecessor of idx
          int sdx = j == corners.size() - 1 ? 0 : j + 1; // successor of idx
 
          cv::Point v1 = corners[sdx] - corners[j];
          cv::Point v2 = corners[pdx] - corners[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.6f)
          {
            validCorners = false;
            break;
          }
        }
    
        isvalid = validCorners;// && NiceHomography(HMatrix);
      }
    };
  }
  catch(cv::Exception &)
  {
    return false;
  }
 
  //clear marker corners
  this->clear();
  
  if (!isvalid)
    return false;
 
  /*
  if ((corners.size() == 4) && (cv::isContourConvex(cv::Mat(corners))))
  {
    bool validCorners = true;
    for (j = 0; j < corners.size(); j++)
    {
      int pdx = j == 0 ? corners.size() - 1 : j - 1; // predecessor of idx
      int sdx = j == corners.size() - 1 ? 0 : j + 1; // successor of idx
 
      cv::Point v1 = corners[sdx] - corners[j];
      cv::Point v2 = corners[pdx] - corners[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)
      {
        validCorners = false;
        break;
      }
    }
    
    isvalid = validCorners;
  }
 
  if (!isvalid)
    return false;
 
  //use the last position for very small moves 
  //interpolate corners between the 2 last frames for better stability
  if(m_lastCorner.empty())
    m_lastCorner = corners;
  else
  {
    // test if a corner move alone
    float c1 = abs ((corners[0].x - m_lastCorner[0].x) + (corners[0].y - m_lastCorner[0].y));
    float c2 = abs ((corners[1].x - m_lastCorner[1].x) + (corners[1].y - m_lastCorner[1].y));
    float c3 = abs ((corners[2].x - m_lastCorner[2].x) + (corners[2].y - m_lastCorner[2].y));
    float c4 = abs ((corners[3].x - m_lastCorner[3].x) + (corners[3].y - m_lastCorner[3].y));
 
    if (c1 >= 1.0f && c2 < 2.0f && c3 < 2.0f && c4 < 2.0f)
      corners[0].x = m_lastCorner[0].x;
 
    if (c2 >= 1.0f && c1 < 2.0f && c3 < 2.0f && c4 < 2.0f)
      corners[1].x = m_lastCorner[1].x;
 
    if (c3 >= 1.0f && c2 < 2.0f && c1 < 2.0f && c4 < 2.0f)
      corners[2].x = m_lastCorner[2].x;
 
    if (c4 >= 1.0f && c2 < 2.0f && c3 < 2.0f && c1 < 2.0f)
      corners[3].x = m_lastCorner[3].x;
 
    float cornerDiff = 0.0f;
    for (j = 0; j < 4; j++)
    {
      cornerDiff += sqrt(((corners[j].x - m_lastCorner[j].x) * (corners[j].x - m_lastCorner[j].x)) +
               ((corners[j].y - m_lastCorner[j].y) * (corners[j].y - m_lastCorner[j].y)));
    }
 
    if((cornerDiff / corners.size()) > 2.5f)
    {
      m_lastCorner = corners;
    }
    else
      corners = m_lastCorner;
  }
  */
 
  for (i = 0; i < corners.size(); i++)
    this->push_back(cv::Point2f(corners[i].x, corners[i].y));
 
  return true;
}
 
/*
void ArFaceMarker::calculateFeaturedExtrinsics(float markerSizeMeters, aruco::CameraParameters& camParams, bool setYPerpendicular)
{
  if(!camParams.isValid())
    throw cv::Exception(9004,"!isValid(): invalid camera parameters. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
  if (!isValid())
    throw cv::Exception(9004,"!isValid(): invalid marker. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
  if (markerSizeMeters<=0)
    throw cv::Exception(9004,"markerSize<=0: invalid markerSize","calculateExtrinsics",__FILE__,__LINE__);
  if (camParams.CameraMatrix.rows == 0 || camParams.CameraMatrix.cols == 0)
    throw cv::Exception(9004,"CameraMatrix is empty","calculateExtrinsics",__FILE__,__LINE__);
 
  cv::Point2f offset = cv::Point2f((float)m_ObjRect.x + ((float)m_ObjRect.width / 2.0f), (float)m_ObjRect.y + ((float)m_ObjRect.height / 2.0f));
  float imgHeight = markerSizeMeters / m_image.rows;
  float imgWidth = imgHeight * (m_image.rows / m_image.cols);
 
  // map points to a cylinder
  std::vector<cv::Point3f> objPoints;
  std::vector<cv::Point2f> prevPoints;
  for (unsigned int i = 0; i < m_ObjPoints.size(); i++)
  {
    if (m_lastFoundPoints[i])
    {
      prevPoints.push_back(m_prevFramePoints[i]);
      cv::Point2f pt = m_ObjPoints[i] - offset;
      objPoints.push_back(UnProjectPoint(cv::Point2f(pt.x * imgWidth, pt.y * imgHeight), offset, imgWidth, imgHeight, imgWidth * 0.5f));
    }
  }
 
  cv::Mat raux, taux;
  cv::solvePnPRansac(objPoints, prevPoints, camParams.CameraMatrix, camParams.Distorsion, raux, taux, true, 100, 20.0f, 0.988f, cv::noArray(), CV_P3P);
  raux.convertTo(Rvec, CV_32F);
  taux.convertTo(Tvec, CV_32F);
 
  //rotate the X axis so that Y is perpendicular to the marker plane
  if (setYPerpendicular)
  {
    cv::Mat R(3,3,CV_32F);
    Rodrigues(Rvec, R);
    //create a rotation matrix for x axis
    float angleRad = CV_PI; // - 180°
 
    cv::Mat RX = cv::Mat::eye(3, 3, CV_32F);
    RX.at<float>(1,1) = cos(angleRad);
    RX.at<float>(1,2) = -sin(angleRad);
    RX.at<float>(2,1) = sin(angleRad);
    RX.at<float>(2,2) = cos(angleRad);
 
    //cv::Mat RY = cv::Mat::eye(3, 3, CV_32F);
    //RY.at<float>(0,0) = cos(angleRad);
    //RY.at<float>(0,2) = -sin(angleRad);
    //RY.at<float>(2,0) = sin(angleRad);
    //RY.at<float>(2,2) = cos(angleRad);
 
    //cv::Mat RZ = cv::Mat::eye(3, 3, CV_32F);
    //RZ.at<float>(0,0) = cos(angleRad);
    //RZ.at<float>(0,1) = -sin(angleRad);
    //RZ.at<float>(1,0) = sin(angleRad);
    //RZ.at<float>(1,1) = cos(angleRad);
 
    //now multiply
    R = R * RX;    
    
    //finally, the the rodrigues back
    cv::Rodrigues(R, Rvec);
  }
 
  //Filter
 
  cv::Mat R1(3, 3, CV_32F);
  cv::Rodrigues(Rvec, R1);
  cv::Point3f euler = rot2euler(R1);
  cv::Point3f pos(Tvec.at<float>(0, 0), Tvec.at<float>(1, 0), Tvec.at<float>(2, 0));
 
  std::vector<cv::Point3f> filtered;
  filtered.push_back(pos);
  filtered.push_back(euler);
 
  m_PosSmoothers.Update(filtered);
  filtered = m_PosSmoothers.GetFilteredPoints();
 
  Tvec.at<float>(0, 0) = filtered[0].x;
  Tvec.at<float>(1, 0) = filtered[0].y;
  Tvec.at<float>(2, 0) = filtered[0].z;
 
  R1 = euler2rot(filtered[1]);
 
  //back to rodrigues
  cv::Rodrigues(R1, Rvec);
}
*/
 
cv::Point3f ArFaceMarker::UnProjectPoint(cv::Point2f point, cv::Point2f offset, float w, float h, float radius)
{
  //center the point at 0,0
  cv::Point2f pc(point.x-w/2,point.y-h/2);
 
  //these are your free parameters
  float f = w;
  float r = w;
 
  float omega = radius;
  float z0 = f - sqrt(r*r-omega*omega);
 
  float zc = (2*z0+sqrt(4*z0*z0-4*(pc.x*pc.x/(f*f)+1)*(z0*z0-r*r)))/(2* (pc.x*pc.x/(f*f)+1)); 
  cv::Point3f final_point(pc.x, pc.y, -zc);
  final_point.x += w/2;
  final_point.y += h/2;
  return final_point;
}
 
void ArFaceMarker::Update(cv::Mat frame, cv::Mat color, aruco::CameraParameters& camparam, bool reverse)
{
  boost::mutex::scoped_lock l(killMutex);
 
  if (!m_bInitialized)
    return;
 
  bool found = (detectFeatured(frame, color, camparam.CamSize)) ? true : false;
  m_bInitialized = found;
  if (found)
  {
    /* Get Matrix*/
    double modelview_matrix[16];
 
    try
    {
      //recalculate Rvec and Tvec with marker size
      calculateFeaturedExtrinsics(m_size, camparam, false);
 
      cv::Mat R(3, 3, CV_32F);
      Rodrigues(Rvec, R);
      //create a rotation matrix for x axis
      float angleRad = CV_PI / 2.0f; // - 90°
 
      //cv::Mat RX = cv::Mat::eye(3, 3, CV_32F);
      //RX.at<float>(1,1) = cos(angleRad);
      //RX.at<float>(1,2) = -sin(angleRad);
      //RX.at<float>(2,1) = sin(angleRad);
      //RX.at<float>(2,2) = cos(angleRad);
 
      //cv::Mat RY = cv::Mat::eye(3, 3, CV_32F);
      //RY.at<float>(0,0) = cos(angleRad);
      //RY.at<float>(0,2) = -sin(angleRad);
      //RY.at<float>(2,0) = sin(angleRad);
      //RY.at<float>(2,2) = cos(angleRad);
 
      cv::Mat RZ = cv::Mat::eye(3, 3, CV_32F);
      RZ.at<float>(0, 0) = cos(angleRad);
      RZ.at<float>(0, 1) = -sin(angleRad);
      RZ.at<float>(1, 0) = sin(angleRad);
      RZ.at<float>(1, 1) = cos(angleRad);
 
      //now multiply
      R = R * RZ;
 
      //finally, the the rodrigues back
      cv::Rodrigues(R, Rvec);
 
      glGetModelViewMatrix(modelview_matrix);
    }
    catch (std::exception&)
    {
      return;
    }
 
    /*if(debugDraw)
    {
    m_objmarker->draw(lastData.image, cv::Scalar(255, 0, 0), 1, true);
    //aruco::CvDrawingUtils::draw3dAxis(image, m_objmarker, arCamParam.camParam);
    }*/
 
    //determine the centroid
    Vector3 pixelPosition(0.0, 0.0, 0.0);
    for (int j = 0; j<4; j++)
    {
      pixelPosition.x += (*this).at(j).x;
      pixelPosition.y += (*this).at(j).y;
    }
 
    pixelPosition.z = sqrt(pow(((*this).at(1).x - (*this).at(0).x), 2) + pow(((*this).at(1).y - (*this).at(0).y), 2));
    pixelPosition.z += sqrt(pow(((*this).at(2).x - (*this).at(1).x), 2) + pow(((*this).at(2).y - (*this).at(1).y), 2));
    pixelPosition.z += sqrt(pow(((*this).at(3).x - (*this).at(2).x), 2) + pow(((*this).at(3).y - (*this).at(2).y), 2));
    pixelPosition.z += sqrt(pow(((*this).at(0).x - (*this).at(3).x), 2) + pow(((*this).at(0).y - (*this).at(3).y), 2));
 
    pixelPosition.x /= 4.;
    pixelPosition.y /= 4.;
    pixelPosition.z /= 4.;
 
    //cv::circle(image, Point(pixelPosition.x, pixelPosition.y),pixelPosition.z/2,cv::Scalar(255,0,255));
 
    SetPixelPosition(pixelPosition);
 
    SetPosition(Vector3(static_cast<float>(reverse ? -modelview_matrix[12] : modelview_matrix[12]), static_cast<float>(modelview_matrix[13]), static_cast<float>(modelview_matrix[14])));
    SetOrientation(BtQuaternion::FromRotationMatrix(modelview_matrix, reverse));
    SetVisible(true);
  }
  else
    SetVisible(false);
 
  m_needUpdate = true;
}