ArFeaturedMarker.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 <scolPlugin.h>
 
#include "ArFeaturedMarker.h"
#include <cstdio>
#include <set>
 
class symTestParallel : public cv::ParallelLoopBody
{
public:
  symTestParallel(const std::vector<std::vector<cv::DMatch> >& _matches1, const std::vector<std::vector<cv::DMatch> >& _matches2, std::vector<std::vector<cv::DMatch> > &_omatchVec)
    : matches1(_matches1),
    matches2(_matches2),
    omatchVec(_omatchVec)
  {}
 
  void operator()(const cv::Range& range) const
  {
    const int start = range.start;
    const int end = range.end;
 
    // for all matches image 1 -> image 2
    for (std::vector<std::vector<cv::DMatch> >::const_iterator matchIterator1 = (matches1.begin() + start); matchIterator1 != (matches1.begin() + end); ++matchIterator1)
    {
      // ignore deleted matches
      if (matchIterator1->empty() || matchIterator1->size() < 2)
        continue;
 
      // for all matches image 2 -> image 1
      for (std::vector<std::vector<cv::DMatch> >::const_iterator
        matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
      {
        // ignore deleted matches
        if (matchIterator2->empty() || matchIterator2->size() < 2)
          continue;
 
        // Match symmetry test
        if ((*matchIterator1)[0].queryIdx ==
          (*matchIterator2)[0].trainIdx &&
          (*matchIterator2)[0].queryIdx ==
          (*matchIterator1)[0].trainIdx)
        {
          // add symmetrical match
          omatchVec[cv::getThreadNum()].push_back(cv::DMatch((*matchIterator1)[0].queryIdx, (*matchIterator1)[0].trainIdx, (*matchIterator1)[0].distance));
          break; // next match in image 1 -> image 2
        }
      }
    }
  }
 
private:
  symTestParallel& operator=(const symTestParallel&); // to quiet MSVC
  std::vector<std::vector<cv::DMatch> > matches1;
  std::vector<std::vector<cv::DMatch> > matches2;
  std::vector<std::vector<cv::DMatch> > &omatchVec;
};
 
RobustMatcher::RobustMatcher() : ratio_(0.6f)
{
  // ORB is the default feature
  /*
  detector_ = cv::ORB::create();
 
  // BruteFroce matcher with Norm Hamming is the default matcher
  matcher_ = cv::makePtr<cv::BFMatcher>((int)cv::NORM_HAMMING, false);
  */
}
 
RobustMatcher::~RobustMatcher()
{
}
 
int RobustMatcher::ratioTest(std::vector<std::vector<cv::DMatch> > &matches)
{
  int removed = 0;
  // for all matches
  for (std::vector<std::vector<cv::DMatch> >::iterator matchIterator = matches.begin(); matchIterator != matches.end(); ++matchIterator)
  {
    // if 2 NN has been identified
    if (matchIterator->size() > 1)
    {
      // check distance ratio
      if ((*matchIterator)[0].distance / (*matchIterator)[1].distance > ratio_)
      {
        matchIterator->clear(); // remove match
        removed++;
      }
    }
    else
    { // does not have 2 neighbours
      matchIterator->clear(); // remove match
      removed++;
    }
  }
  return removed;
}
 
void RobustMatcher::symmetryTest(const std::vector<std::vector<cv::DMatch> >& matches1,
  const std::vector<std::vector<cv::DMatch> >& matches2,
  std::vector<cv::DMatch>& symMatches)
{
  std::vector<std::vector<cv::DMatch> > symParallel(cv::getNumThreads());
 
  //do it parallel
  cv::parallel_for_(cv::Range(0, matches1.size()), symTestParallel(matches1, matches2, symParallel));
 
  //merge vectors
  for (unsigned int i = 0; i < symParallel.size(); i++)
  {
    symMatches.insert(symMatches.end(), symParallel[i].begin(), symParallel[i].end());
  }
}
 
void RobustMatcher::robustMatch(const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
  std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& descriptors_model, const cv::Mat& mask)
{
  cv::Mat descriptors_frame;
 
  // 1a. Detection of the features
  detector_->detectAndCompute(frame, mask, keypoints_frame, descriptors_frame);
 
  // 2. Match the two image descriptors
  std::vector<std::vector<cv::DMatch> > matches12, matches21;
 
  // 2a. From image 1 to image 2
  matcher_->knnMatch(descriptors_frame, descriptors_model, matches12, 2); // return 2 nearest neighbours
 
  // 2b. From image 2 to image 1
  matcher_->knnMatch(descriptors_model, descriptors_frame, matches21, 2); // return 2 nearest neighbours
 
  // 3. Remove matches for which NN ratio is > than threshold
  // clean image 1 -> image 2 matches
  ratioTest(matches12);
  // clean image 2 -> image 1 matches
  ratioTest(matches21);
 
  // 4. Remove non-symmetrical matches
  symmetryTest(matches12, matches21, good_matches);
}
 
void RobustMatcher::fastRobustMatch(const cv::Mat& frame, std::vector<cv::DMatch>& good_matches,
  std::vector<cv::KeyPoint>& keypoints_frame, const cv::Mat& mask)
{
  cv::Mat descriptors_frame;
  good_matches.clear();
 
  // 1a. Detection of the features
  detector_->detectAndCompute(frame, mask, keypoints_frame, descriptors_frame);
 
  // 2. Match the two image descriptors
  std::vector<std::vector<cv::DMatch> > matches;
  matcher_->knnMatch(descriptors_frame, matches, 2);
 
  // 3. Remove matches for which NN ratio is > than threshold
  ratioTest(matches);
 
  // 4. Fill good matches container
  for (std::vector<std::vector<cv::DMatch> >::iterator matchIterator = matches.begin(); matchIterator != matches.end(); ++matchIterator)
  {
    if (!matchIterator->empty()) good_matches.push_back((*matchIterator)[0]);
  }
}
 
bool RobustMatcher::addTrainData(const cv::Mat& frame, std::vector<cv::KeyPoint> &marker_keypoints)
{
  //if (marker_keypoints.size() > 2000)
    //return false;
 
  cv::Mat descriptors_marker;
  std::vector<cv::KeyPoint> keypoints_marker;
  std::vector<cv::Mat> descMat;
 
  try
  {
    // 1a. Detection of the features
    traindetector_->detectAndCompute(frame, cv::Mat(), keypoints_marker, descriptors_marker);
    if (keypoints_marker.size() < 30)
      return false;
 
    descMat = matcher_->getTrainDescriptors();
    matcher_->clear();
 
    marker_keypoints.insert(marker_keypoints.end(), keypoints_marker.begin(), keypoints_marker.end());
    descMat.push_back(descriptors_marker);
 
    if (descMat.size() > 1)
    {
      cv::vconcat(descMat, descriptors_marker);
      descMat.clear();
      descMat.push_back(descriptors_marker);
    }
 
    matcher_->add(descMat);
    matcher_->train();
    return true;
  }
  catch (std::exception &)
  {
    return false;
  }
}
 
ArFeaturedMarker::ArFeaturedMarker(std::string filePath, unsigned int nid, float size, unsigned int maxFeatures) : ArMarker(nid, size, ArMarker::AR_FFT_MARKER)
{
  m_file = filePath;
  m_maxFeatures = maxFeatures;
 
  if (!m_file.empty())
  {
#ifdef ANDROID
    long lSize = 0;
    char* buff = 0;
 
    FILE* pFile = fopen(m_file.c_str(), "rb");
    if (pFile)
    {
      // obtain file size.
      fseek(pFile, 0, SEEK_END);
      lSize = ftell(pFile);
      rewind(pFile);
 
      // allocate memory to contain the whole file.
      buff = (char*)malloc(lSize);
      if (buff)
      {
        // copy the file into the buffer.
        fread(buff, 1, lSize, pFile);
        std::vector<char> data(buff, buff + lSize);
 
        // terminate
        fclose(pFile);
 
        m_image = cv::imdecode(data, cv::IMREAD_GRAYSCALE);
        free(buff);
      }
      if (m_image.empty())
        MMechostr(MSKDEBUG, ">>>>> Picture not loaded ko\n");
      else
        MMechostr(MSKDEBUG, ">>>>> Picture loaded ok\n");
    }
    else
    {
      MMechostr(MSKDEBUG, ">>>>> Picture failed to open : %s\n", filePath.c_str());
    }
#else
    m_image = cv::imread(m_file, cv::IMREAD_GRAYSCALE);
#endif
  }
 
  CommonConstructor();
}
 
ArFeaturedMarker::ArFeaturedMarker(cv::Mat tpl, unsigned int nid, float size, unsigned int maxFeatures) : ArMarker(nid, size, ArMarker::AR_FFT_MARKER)
{
  m_file = "";
  m_maxFeatures = maxFeatures;
 
  cv::cvtColor(tpl, m_image, cv::COLOR_RGB2GRAY);
 
  CommonConstructor();
}
 
//for face detection
ArFeaturedMarker::ArFeaturedMarker(unsigned int nid, float size) : ArMarker(nid, size, ArMarker::AR_FACE_MARKER)
{
  m_file = "";
  m_maxFeatures = 400;
 
  CommonConstructor();
}
 
void ArFeaturedMarker::SetTrackedImage(cv::Mat tpl)
{
  tpl.copyTo(m_image);
 
  if (!m_image.empty())
  {
    // resize the marker for bigger detection and this avoid an infinite loop on pyramid detection on small pictures
    cv::Size nsize;
 
    nsize.width = m_image.cols * 2;
    nsize.height = m_image.rows * 2;
 
    cv::resize(m_image, m_image, nsize, 0, 0, cv::INTER_CUBIC);
    //cv::blur(m_image, m_image, cv::Size(2, 2));
    //imshow("marker", m_image);
 
    initTrackingPicture();
  }
}
 
void ArFeaturedMarker::CommonConstructor()
{
  m_detector.release();
  m_matcher.release();
  m_robustMatcher = 0;
  m_registerNextFrame = false;
  m_lastFound = false;
  m_KalmanInit = false;
  m_lastTick = 0;
  m_minMatches = 6;
  m_HomographyThreshold = 8;
 
  m_CornerSmoothers.Init(0.2f, 0.2f, 0.2f, 25.0f, 35.0f);
 
  //#ifdef ANDROID
  //  m_scale = cv::Point2f(0.5f, 0.5f);
  //#else
  //  m_scale = cv::Point2f(1.0f, 1.0f);
  //#endif
 
  int msize = 1024;
 
  if (!m_image.empty())
  {
    // resize the marker for bigger detection and this avoid an infinite loop on pyramid detection on small pictures
    cv::Size nsize;
    if (m_image.rows >= m_image.cols)
    {
      nsize.width = (int)(((float)m_image.cols * (float)msize) / (float)m_image.rows);
      nsize.height = msize;
    }
    else
    {
      nsize.width = msize;
      nsize.height = (int)(((float)m_image.rows * (float)msize) / (float)m_image.cols);
    }
 
    cv::resize(m_image, m_image, nsize, 0, 0, cv::INTER_CUBIC);
    //cv::equalizeHist(m_image, m_image);
    //cv::blur(m_image, m_image, cv::Size(2, 2));
 
    //imshow("marker", m_image);
 
    initTrackingPicture();
  }
}
 
ArFeaturedMarker::~ArFeaturedMarker()
{
  std::vector<int> empty(2);
  empty[0] = 0; empty[1] = 0;
  m_image = cv::Mat(empty);
  m_pcorners.clear();
 
  if (m_robustMatcher)
  {
    delete(m_robustMatcher);
    m_robustMatcher = 0;
  }
 
  if (!m_detector.empty())
  {
    m_detector.release();
  }
 
  if (!m_matcher.empty())
  {
    m_matcher.release();
  }
}
 
bool ArFeaturedMarker::initTrackingPicture()
{
  m_pcorners.clear();
  m_lastTick = 0;
  if (m_robustMatcher)
  {
    delete(m_robustMatcher);
    m_robustMatcher = 0;
  }
 
  if (!m_detector.empty())
  {
    m_detector.release();
  }
 
  if (!m_matcher.empty())
  {
    m_matcher.release();
  }
 
  m_dmask.setTo(255);
 
  m_pcorners.clear();
  m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
  m_pcorners.push_back(cv::Point2f(static_cast<float>(m_image.cols), 0.0f));
  m_pcorners.push_back(cv::Point2f(static_cast<float>(m_image.cols), static_cast<float>(m_image.rows)));
  m_pcorners.push_back(cv::Point2f(0.0f, static_cast<float>(m_image.rows)));
 
  //m_matcher = cv::makePtr<cv::BFMatcher>(cv::NORM_HAMMING);
  cv::Ptr<cv::flann::IndexParams> indexParams = cv::makePtr<cv::flann::LshIndexParams>(6, 12, 1); // instantiate LSH index parameters
  cv::Ptr<cv::flann::SearchParams> searchParams = cv::makePtr<cv::flann::SearchParams>(50);       // instantiate flann search parameters
  m_matcher = cv::makePtr<cv::FlannBasedMatcher>(indexParams, searchParams);
 
  m_robustMatcher = new RobustMatcher();
  m_robustMatcher->setDescriptorMatcher(m_matcher);
  return trainMatcher();
}
 
void ArFeaturedMarker::RegisterNextFrame(cv::Point point, cv::Size size)
{
  m_cropPos = point;
  m_cropSize = size;
  m_registerNextFrame = true;
}
 
bool ArFeaturedMarker::trainMatcher()
{
  m_matcher->clear();
  m_trainKeypoints.clear();
  if (!m_detector.empty())
  {
    m_detector.release();
  }
 
  //m_detector = cv::ORB::create(m_maxFeatures, 1.0f, 1, 8, 0, 2, 1, 8);
  //m_extractor = cv::ORB::create(m_maxFeatures, 1.35f, 24, 31, 0, 2, 1, 31);
 
  float threshold = 0.003f / ((float)m_maxFeatures / 100.0f);
  m_detector = cv::AKAZE::create(cv::AKAZE::DESCRIPTOR_MLDB, 0, 3, threshold, 1, 1);
 
  m_robustMatcher->setFeatureDetector(m_detector);
 
  std::vector<cv::Mat> descMat;
  //cv::Ptr<cv::ORB> sdetector = cv::ORB::create(500000, 1.2f, 16, 31, 0, 2, 1, 16);
 
  cv::Ptr<cv::AKAZE> sdetector = cv::AKAZE::create(cv::AKAZE::DESCRIPTOR_MLDB, 0, 3, threshold, 32, 4, cv::KAZE::DIFF_PM_G2);
  m_robustMatcher->setTrainFeatureDetector(sdetector);
  m_robustMatcher->addTrainData(m_image, m_trainKeypoints);
 
  //cv::Mat eq;
  //cv::equalizeHist(m_image, eq);
  //m_robustMatcher->addTrainData(eq, m_trainKeypoints);
 
  /*
  cv::Mat warp;
  cv::Mat warped;
  cv::Mat hmat;
 
  rotateImage(m_image, warp, hmat, 45, 0, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, -45, 0, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, 0, 45, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, 0, -45, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, 85, 0, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, -85, 0, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, 0, 85, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  rotateImage(m_image, warp, hmat, 0, -85, 0, 0, 0, 2000, 500);
  cv::warpPerspective(warp, warped, hmat, cv::Size(warp.cols, warp.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  m_robustMatcher->addTrainData(warped, m_trainKeypoints);
 
  */
 
  if (m_trainKeypoints.size() < 50)
    return false;
 
  return true;
}
 
bool ArFeaturedMarker::GetPointsFromMatches(const std::vector<cv::KeyPoint> &querykeypoints, const std::vector<cv::KeyPoint> &trainKeypoints, std::vector<cv::DMatch> matches, std::vector<cv::Point2f> &trainPoints, std::vector<cv::Point2f> &framePoints)
{
  if (matches.size() >= m_minMatches)
  {
    for (unsigned int i = 0; i < matches.size(); i++)
    {
      cv::DMatch& match = matches[i];
      trainPoints.push_back(trainKeypoints[match.trainIdx].pt);
      framePoints.push_back(querykeypoints[match.queryIdx].pt);
    }
    return true;
  }
  else
    return false;
}
 
bool ArFeaturedMarker::GetHomography(std::vector<cv::Point2f> &trainPoints, std::vector<cv::Point2f> &framePoints, cv::Mat &HMatrix)
{
  try
  {
    //std::vector<uchar> status;
    HMatrix = findHomography(trainPoints, framePoints, cv::FM_RANSAC, m_HomographyThreshold);// , status);
 
    /*
    std::vector<cv::Point2f> nGoodFramePoints;
    std::vector<cv::Point2f> nTrainPoints;
    unsigned int nbMatches = 0;
    for (unsigned int i = 0; i < trainPoints.size(); i++)
    {
      if (status[i] > 0)
      {
        nbMatches++;
        nTrainPoints.push_back(trainPoints[i]);
        nGoodFramePoints.push_back(framePoints[i]);
      }
    }
 
    trainPoints = nTrainPoints;
    framePoints = nGoodFramePoints;
    */
  }
  catch (cv::Exception &e)
  {
    //std::string wt = e.what();
    //error
  }
 
  if (HMatrix.empty())
  {
    m_dmask.setTo(255);
    return false;
  }
  else
    return true;
}
 
bool ArFeaturedMarker::GetCorners(cv::Mat &HMatrix)
{
  unsigned int i;
  std::vector<cv::Point2f> corners;
  cv::Mat transformedPoints;
  bool isvalid = false;
 
  cv::perspectiveTransform(cv::Mat(m_pcorners), transformedPoints, HMatrix);
 
  for (i = 0; i < m_pcorners.size(); i++)
    corners.push_back(transformedPoints.at<cv::Point2f>(i, 0));
 
  //clear marker corners
  this->clear();
 
  if ((corners.size() == 4) && (checkRectShape(corners)))
  {
    isvalid = NiceHomography(HMatrix);
  }
 
  if (!isvalid)
  {
    m_dmask.setTo(255);
    return false;
  }
 
  m_CornerSmoothers.Update(corners);
  corners = m_CornerSmoothers.GetFilteredPoints();
 
  for (i = 0; i < corners.size(); i++)
  {
    this->push_back(cv::Point2f(corners[i].x, corners[i].y));
  }
 
  //keep last marker zone for next detection
  m_dmask = createMask(m_dmask.size(), corners);
  return true;
}
 
bool ArFeaturedMarker::detectMotionFlow(cv::Mat &frame, cv::Mat &color, const std::vector<cv::Point2f> &trainPoints, const std::vector<cv::Point2f> &framePoints, std::vector<cv::Point2f> &gtrainPoints, std::vector<cv::Point2f> &gframePoints)
{
  std::vector<cv::Point2f> newFramePoints;
  std::vector<uchar> status;
  std::vector<float> err;
  cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 200, 0.02f);
  cv::Size winSize(41, 41);
 
  cv::buildOpticalFlowPyramid(frame, m_nextPyr, winSize, 8, true);
 
  if (framePoints.empty() || m_prevPyr.empty())
  {
    m_prevPyr.swap(m_nextPyr);
    return false;
  }
 
  try
  {
    cv::calcOpticalFlowPyrLK(m_prevPyr, m_nextPyr, framePoints, newFramePoints, status, err, winSize, 8, termcrit, cv::OPTFLOW_FARNEBACK_GAUSSIAN, 0.001f);
    m_prevPyr.swap(m_nextPyr);
 
    unsigned int nbMatches = 0;
    for (unsigned int i = 0; i < trainPoints.size(); i++)
    {
      //cv::circle(color, framePoints[i], 4, cv::Scalar(255, 0, 0), 1, 8, 0);
      //cv::circle(color, newFramePoints[i], 4, cv::Scalar(0, 255, 0), 1, 8, 0);
      //cv::line(color, framePoints[i], newFramePoints[i], cv::Scalar(255, 0, 0), 1, CV_AA);
 
      if (status[i] == 1 && err[i] < 150.0f)
      {
        nbMatches++;
        gtrainPoints.push_back(trainPoints[i]);
        gframePoints.push_back(newFramePoints[i]);
      }
    }
 
    if (nbMatches >= m_minMatches)
      return true;
  }
  catch(std::exception &e)
  {
    std::string wt = e.what();
    return false;
  }
 
  return false;
}
 
bool ArFeaturedMarker::GetWarpedMarker(cv::Mat &image)
{
  boost::mutex::scoped_lock l(m_mutex);
  m_warped.copyTo(image);
 
  if (image.empty())
    return false;
  else
    return true;
}
 
/**** feature detection *****/
bool ArFeaturedMarker::detectFeatured(cv::Mat &frame, cv::Mat &color, cv::Size camsize)
{
  if (m_lastFrameSize.width != frame.cols || m_lastFrameSize.height != frame.rows || m_registerNextFrame)
  {
    m_prevPyr.clear();
    m_prevFramePoints.clear();
    m_prevTrainPoints.clear();
    m_dmask.setTo(255);
    m_lastTick = 0;
 
    m_CornerSmoothers.Reset();
  }
 
  m_lastFrameSize = cv::Size(frame.cols, frame.rows);
  cv::Mat HMatrix;
 
  if (m_registerNextFrame && camsize.width != 0 && camsize.height != 0 && m_cropSize.width > 1 && m_cropSize.height > 1)
  {
    //crop the picture with the coords
    cv::Point2f scale = cv::Point2f((float)frame.cols / (float)camsize.width, (float)frame.rows / (float)camsize.height);
 
    m_cropPos.x = (int)((float)m_cropPos.x * scale.x);
    m_cropPos.y = (int)((float)m_cropPos.y * scale.y);
    m_cropSize.width = (int)((float)m_cropSize.width * scale.x);
    m_cropSize.height = (int)((float)m_cropSize.height * scale.x);
 
    int msize = 1024;
    try
    {
      cv::Mat crop(frame, cv::Rect(m_cropPos.x, m_cropPos.y, m_cropSize.width, m_cropSize.height)); // NOTE: this will only give you a reference to the ROI of the original data
      cv::Size nsize;
      if (crop.rows >= crop.cols)
      {
        nsize.width = (int)(((float)crop.cols * (float)msize) / (float)crop.rows);
        nsize.height = msize;
      }
      else
      {
        nsize.width = msize;
        nsize.height = (int)(((float)crop.rows * (float)msize) / (float)crop.cols);
      }
      cv::Mat marker;
      cv::resize(crop, marker, nsize, 0, 0, cv::INTER_CUBIC);
      //cv::blur(marker, marker, cv::Size(2, 2));
 
      m_registerNextFrame = (m_robustMatcher->addTrainData(marker, m_trainKeypoints)) ? false : true;
    }
    catch (std::exception &)
    {
      //could not set current frame
      MMechostr(MSKDEBUG, "detectFeatured : failed to set the current frame as marker.\n");
    }
  }
 
  if (frame.empty() || m_image.empty())
    return false;
 
  if (m_lastFound)
  {
    // try with optical flow
    std::vector<cv::Point2f> gFramePoints;
    std::vector<cv::Point2f> gTrainPoints;
 
    bool motion = detectMotionFlow(frame, color, m_prevTrainPoints, m_prevFramePoints, gTrainPoints, gFramePoints);
 
    //try with motion flow
    if (motion && !gFramePoints.empty() && !gTrainPoints.empty())
    {
      if ((m_lastFound = GetHomography(gTrainPoints, gFramePoints, HMatrix)))
      {
        m_lastFound = GetCorners(HMatrix);
 
        cv::Mat prev = cv::Mat(m_prevTrainPoints, false);
        cv::Mat next;
        cv::perspectiveTransform(prev, next, HMatrix);
        next.copyTo(cv::Mat(m_prevFramePoints, false));
      }
    }
    else
    {
      m_lastFound = false;
    }
  }
 
  if (!m_lastFound)
  {
    //init the detection mask
    if ((m_dmask.rows == 0) || (m_dmask.cols == 0) || m_dmask.empty() || (m_dmask.rows != frame.rows) || m_dmask.cols != frame.cols)
    {
      m_dmask = cv::Mat(frame.rows, frame.cols, CV_8UC1);
      m_dmask.setTo(255);
    }
 
    std::vector<cv::KeyPoint> keypoints;
    std::vector<cv::DMatch> matches;
    try
    {
      m_robustMatcher->fastRobustMatch(frame, matches, keypoints, m_dmask);
 
      std::vector<cv::Point2f> trainPoints;
      std::vector<cv::Point2f> framePoints;
 
      if ((m_lastFound = GetPointsFromMatches(keypoints, m_trainKeypoints, matches, trainPoints, framePoints)))
      {
        m_prevFramePoints = framePoints;
        m_prevTrainPoints = trainPoints;
 
        /*
        cv::Size winSize = cv::Size(5, 5);
        cv::Size zeroZone = cv::Size(-1, -1);
        cv::TermCriteria criteria = cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001);
        cv::cornerSubPix(frame, m_prevFramePoints, winSize, zeroZone, criteria);
        cv::cornerSubPix(m_image, m_prevTrainPoints, winSize, zeroZone, criteria);
        */
 
        if ((m_lastFound = GetHomography(trainPoints, framePoints, HMatrix)) && (m_lastFound = GetCorners(HMatrix)))
        {
          // reset motion flow
          std::vector<cv::Point2f> gFramePoints;
          std::vector<cv::Point2f> gTrainPoints;
          m_prevPyr.clear();
          detectMotionFlow(frame, color, m_prevTrainPoints, m_prevFramePoints, gFramePoints, gTrainPoints);
 
          // get warped detected marker
          boost::mutex::scoped_lock l(m_mutex);
          cv::warpPerspective(color, m_warped, HMatrix, cv::Size(m_image.cols, m_image.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
        }
      }
    }
    catch (std::exception&)
    {
      m_lastFound = false;
    }
  }
 
  //reset mask
  if (!m_lastFound)
  {
    m_KalmanInit = false;
    m_prevPyr.clear();
    m_prevFramePoints.clear();
    m_prevTrainPoints.clear();
    m_dmask.setTo(255);
    m_lastTick = 0;
    m_CornerSmoothers.Reset();
  }
  /*
  // too slow on android
  else
  {
    // get warped detected marker
    boost::mutex::scoped_lock l(m_mutex);
    cv::warpPerspective(color, m_warped, HMatrix, cv::Size(m_image.cols, m_image.rows), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
  }
  */
 
  return m_lastFound;
}
 
void ArFeaturedMarker::calculateFeaturedExtrinsics(float markerSizeMeters, aruco::CameraParameters& camParams, bool setYPerperdicular)
{
  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__);
 
  float halfSizeX = 0.0f;
  float halfSizeY = 0.0f;
  if (m_image.rows >= m_image.cols)
  {
    halfSizeX = markerSizeMeters / 2.0f;
    halfSizeY = ((m_image.cols * markerSizeMeters) / m_image.rows) / 2.0f;
  }
  else
  {
    halfSizeX = ((m_image.rows * markerSizeMeters) / m_image.cols) / 2.0f;
    halfSizeY = markerSizeMeters / 2.0f;
  }
 
  cv::Mat ObjPoints(4, 3, CV_32FC1);
  ObjPoints.at<float>(1, 0) = -halfSizeX;
  ObjPoints.at<float>(1, 1) = halfSizeY;
  ObjPoints.at<float>(1, 2) = 0;
  ObjPoints.at<float>(2, 0) = halfSizeX;
  ObjPoints.at<float>(2, 1) = halfSizeY;
  ObjPoints.at<float>(2, 2) = 0;
  ObjPoints.at<float>(3, 0) = halfSizeX;
  ObjPoints.at<float>(3, 1) = -halfSizeY;
  ObjPoints.at<float>(3, 2) = 0;
  ObjPoints.at<float>(0, 0) = -halfSizeX;
  ObjPoints.at<float>(0, 1) = -halfSizeY;
  ObjPoints.at<float>(0, 2) = 0;
 
  cv::Mat ImagePoints(4, 2, CV_32FC1);
 
  cv::Point2f scale = cv::Point2f((float)m_lastFrameSize.width / (float)camParams.CamSize.width, (float)m_lastFrameSize.height / (float)camParams.CamSize.height);
 
  //Set image points from the marker
  for (int c = 0; c < 4; c++)
  {
    (*this)[c % 4].x = ((*this)[c % 4].x) * (1.0f / scale.x);
    (*this)[c % 4].y = ((*this)[c % 4].y) * (1.0f / scale.y);
 
    ImagePoints.at<float>(c, 0) = ((*this)[c % 4].x);
    ImagePoints.at<float>(c, 1) = ((*this)[c % 4].y);
  }
 
  cv::Mat raux, taux;
  cv::solvePnP(ObjPoints, ImagePoints, camParams.CameraMatrix, camParams.Distorsion, raux, taux);
  //cv::solvePnPRansac(ObjPoints, ImagePoints, camParams.CameraMatrix, camParams.Distorsion, raux, taux, false, 100, 9.0f, 0.95, 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 (setYPerperdicular)
    rotateXAxis(Rvec);
}
 
void ArFeaturedMarker::Update(cv::Mat frame, cv::Mat color, aruco::CameraParameters& camparam, bool reverse)
{
  boost::mutex::scoped_lock l(killMutex);
  m_bUpdating = true;
  //bool found = (m_visible && m_fmarker->detectMotionFlow(m_lastFrame)) ? true : m_fmarker->detectFeatured(m_lastFrame);
  bool found = detectFeatured(frame, color, camparam.CamSize);
  if (found)
  {
    m_bUpdating = false;
 
    //Get Matrix
    double modelview_matrix[16];
 
    try
    {
      //recalculate Rvec and Tvec with marker size
      calculateFeaturedExtrinsics(m_size, camparam);
      glGetModelViewMatrix(modelview_matrix);
    }
    catch (std::exception&)
    {
      return;
    }
 
    //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;
  m_bUpdating = false;
}