ArTkMarker.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 "ArTkMarker.h"
#include "ArManager.h"
#include <cstdio>
 
#include <AR/ar.h>
#include <AR2/config.h>
#include <AR2/imageFormat.h>
#include <AR2/imageSet.h>
#include <AR2/featureSet.h>
#include <AR2/util.h>
 
#include <boost/filesystem.hpp>
 
#include <set>
 
#define MIN_TRACKED_MARKER_SIZE 240
 
extern int AR_MARKER_TRAINNED_CB;
 
bool NFTDataLoader::LoadData(boost::filesystem::path path, KpmRefDataSet** refDataSetPtr, AR2SurfaceSetT** surfaceSetPtr)
{
  if (LoadRefDataSet(path, refDataSetPtr))
    if (LoadFeatureSet(path, surfaceSetPtr))
      return true;
 
  return false;
}
 
bool NFTDataLoader::LoadRefDataSet(boost::filesystem::path path, KpmRefDataSet** refDataSetPtr)
{
  KpmRefDataSet *refDataSet = NULL;
  FILE *fp;
  int i, j;
 
  if (path.empty() || !refDataSetPtr)
  {
    return false;
  }
 
  path.replace_extension(".fset3");
  fp = fopen(path.generic_string().c_str(), "rb");
  if (!fp)
  {
    return false;
  }
 
  arMallocClear(refDataSet, KpmRefDataSet, 1);
 
  if (fread(&(refDataSet->num), sizeof(int), 1, fp) != 1) goto bailBadRead;
  if (refDataSet->num <= 0) goto bailBadRead;
 
  arMalloc(refDataSet->refPoint, KpmRefData, refDataSet->num); // each KpmRefData = 68 floats, 3 ints = 284 bytes.
  for (i = 0; i < refDataSet->num; i++)
  {
    if (fread(&(refDataSet->refPoint[i].coord2D), sizeof(KpmCoord2D), 1, fp) != 1) goto bailBadRead;
    if (fread(&(refDataSet->refPoint[i].coord3D), sizeof(KpmCoord2D), 1, fp) != 1) goto bailBadRead;
    if (fread(&(refDataSet->refPoint[i].featureVec), sizeof(FreakFeature), 1, fp) != 1) goto bailBadRead;
    if (fread(&(refDataSet->refPoint[i].pageNo), sizeof(int), 1, fp) != 1) goto bailBadRead;
    if (fread(&(refDataSet->refPoint[i].refImageNo), sizeof(int), 1, fp) != 1) goto bailBadRead;
  }
 
  if (fread(&(refDataSet->pageNum), sizeof(int), 1, fp) != 1) goto bailBadRead;
  if (refDataSet->pageNum <= 0)
  {
    refDataSet->pageInfo = NULL;
    goto bailBadRead;
  }
 
  arMalloc(refDataSet->pageInfo, KpmPageInfo, refDataSet->pageNum);
  for (i = 0; i < refDataSet->pageNum; i++)
  {
    if (fread(&(refDataSet->pageInfo[i].pageNo), sizeof(int), 1, fp) != 1) goto bailBadRead;
    if (fread(&(refDataSet->pageInfo[i].imageNum), sizeof(int), 1, fp) != 1) goto bailBadRead;
 
    j = refDataSet->pageInfo[i].imageNum;
    arMalloc(refDataSet->pageInfo[i].imageInfo, KpmImageInfo, j);
 
    if (fread(refDataSet->pageInfo[i].imageInfo, sizeof(KpmImageInfo), j, fp) != j) goto bailBadRead;
  }
 
  *refDataSetPtr = refDataSet;
  if (fp)
    fclose(fp);
 
  return true;
 
bailBadRead:
  if (refDataSet)
    kpmDeleteRefDataSet(&refDataSet);
  if (fp)
    fclose(fp);
  return false;
}
 
bool NFTDataLoader::LoadFeatureSet(boost::filesystem::path path, AR2SurfaceSetT** surfaceSetPtr)
{
  int i, j, k;
  AR2SurfaceSetT* surfaceSet = NULL;
  FILE *fp = NULL;
 
  if (path.empty() || !surfaceSetPtr)
  {
    return false;
  }
 
  arMalloc(surfaceSet, AR2SurfaceSetT, 1);
 
  surfaceSet->num = 1;
  surfaceSet->contNum = 0;
  arMalloc(surfaceSet->surface, AR2SurfaceT, surfaceSet->num);
 
  for (i = 0; i < surfaceSet->num; i++)
  {
    surfaceSet->surface[i].imageSet = GetImageSet(path);
    if (surfaceSet->surface[i].imageSet == NULL)
    {
      goto bailBadRead;
    }
 
    surfaceSet->surface[i].featureSet = ReadFeatureSet(path);
    if (surfaceSet->surface[i].featureSet == NULL)
    {
      goto bailBadRead;
    }
    surfaceSet->surface[i].markerSet = NULL;
 
    for (j = 0; j < 3; j++)
    {
      for (k = 0; k < 4; k++)
      {
        surfaceSet->surface[i].trans[j][k] = (j == k) ? 1.0f : 0.0f;
      }
    }
    arUtilMatInvf((const float(*)[4])surfaceSet->surface[i].trans, surfaceSet->surface[i].itrans);
    surfaceSet->surface[i].jpegName = 0;
  }
 
  if (fp)
    fclose(fp);
 
  if (i < surfaceSet->num)
    goto bailBadRead;
 
  *surfaceSetPtr = surfaceSet;
  return true;
 
bailBadRead:
  if (surfaceSet && surfaceSet->surface)
    free(surfaceSet->surface);
 
  if (surfaceSet)
    free(surfaceSet);
 
  if (fp)
    fclose(fp);
  return false;
}
 
AR2ImageSetT* NFTDataLoader::GetImageSet(boost::filesystem::path path)
{
  AR2JpegImageT *jpgImage;
  AR2ImageSetT  *imageSet;
  FILE *fp = NULL;
  float dpi;
  int i, k1;
 
  path.replace_extension(".iset");
  if ((fp = fopen(path.generic_string().c_str(), "rb")) == NULL)
  {
    goto bailBadRead;
  }
 
  arMalloc(imageSet, AR2ImageSetT, 1);
  if (fread(&(imageSet->num), sizeof(imageSet->num), 1, fp) != 1 || imageSet->num <= 0)
  {
    free(imageSet);
    goto bailBadRead;
  }
 
  arMalloc(imageSet->scale, AR2ImageT*, imageSet->num);
  arMalloc(imageSet->scale[0], AR2ImageT, 1);
 
  jpgImage = ar2ReadJpegImage2(fp);
  if (jpgImage == NULL || jpgImage->nc != 1)
  {
    free(imageSet->scale[0]);
    free(imageSet->scale);
    free(imageSet);
 
    if (jpgImage == NULL)
    {
      rewind(fp);
      return LoadRawImageSet(fp);
    }
 
    free(jpgImage);
    goto bailBadRead;
  }
 
  imageSet->scale[0]->xsize = jpgImage->xsize;
  imageSet->scale[0]->ysize = jpgImage->ysize;
  imageSet->scale[0]->dpi = jpgImage->dpi;
  imageSet->scale[0]->imgBW = jpgImage->image;
  free(jpgImage);
 
  // Minify for the other scales.
  // First, find the list of scales we wrote into the file.
  fseek(fp, (long)(-(int)sizeof(dpi)*(imageSet->num - 1)), SEEK_END);
  for (i = 1; i < imageSet->num; i++)
  {
    if (fread(&dpi, sizeof(dpi), 1, fp) != 1)
    {
      for (k1 = 0; k1 < i; k1++)
      {
        free(imageSet->scale[k1]->imgBW);
        free(imageSet->scale[k1]);
      }
      free(imageSet->scale);
      goto bailBadRead;
    }
 
    imageSet->scale[i] = GetImageLayer(imageSet->scale[0], dpi);
    if (imageSet->scale[i] == NULL)
    {
      for (k1 = 0; k1 < i; k1++)
      {
        free(imageSet->scale[k1]->imgBW);
        free(imageSet->scale[k1]);
      }
      free(imageSet->scale);
      goto bailBadRead;
    }
  }
 
  if (fp)
    fclose(fp);
 
  return imageSet;
 
bailBadRead:
  if (fp)
    fclose(fp);
  return NULL;
}
 
AR2ImageSetT* NFTDataLoader::LoadRawImageSet(FILE* fp)
{
  AR2ImageSetT* imageSet;
  int i, k;
 
  arMalloc(imageSet, AR2ImageSetT, 1);
 
  if (fread(&(imageSet->num), sizeof(imageSet->num), 1, fp) != 1 || imageSet->num <= 0)
  {
    goto bail;
  }
 
  arMalloc(imageSet->scale, AR2ImageT*, imageSet->num);
  for (i = 0; i < imageSet->num; i++)
  {
    arMalloc(imageSet->scale[i], AR2ImageT, 1);
  }
 
  for (i = 0; i < imageSet->num; i++)
  {
    if (fread(&(imageSet->scale[i]->xsize), sizeof(imageSet->scale[i]->xsize), 1, fp) != 1)
    {
      for (k = 0; k < i; k++)
      {
        free(imageSet->scale[k]->imgBW);
      }
      for (k = 0; k < imageSet->num; k++) free(imageSet->scale[k]);
      goto bail1;
    }
    if (fread(&(imageSet->scale[i]->ysize), sizeof(imageSet->scale[i]->ysize), 1, fp) != 1)
    {
      for (k = 0; k < i; k++)
      {
        free(imageSet->scale[k]->imgBW);
      }
      for (k = 0; k < imageSet->num; k++) free(imageSet->scale[k]);
      goto bail1;
    }
    if (fread(&(imageSet->scale[i]->dpi), sizeof(imageSet->scale[i]->dpi), 1, fp) != 1)
    {
      for (k = 0; k < i; k++)
      {
        free(imageSet->scale[k]->imgBW);
      }
      for (k = 0; k < imageSet->num; k++) free(imageSet->scale[k]);
      goto bail1;
    }
 
    arMalloc(imageSet->scale[i]->imgBW, ARUint8, imageSet->scale[i]->xsize * imageSet->scale[i]->ysize);
 
    if (fread(imageSet->scale[i]->imgBW, sizeof(ARUint8), imageSet->scale[i]->xsize * imageSet->scale[i]->ysize, fp)
      != imageSet->scale[i]->xsize * imageSet->scale[i]->ysize)
    {
      for (k = 0; k <= i; k++)
      {
        free(imageSet->scale[k]->imgBW);
      }
      for (k = 0; k < imageSet->num; k++) free(imageSet->scale[k]);
      goto bail1;
    }
  }
 
  fclose(fp);
  return imageSet;
 
bail1:
  free(imageSet->scale);
bail:
  free(imageSet);
  fclose(fp);
  return NULL;
}
 
AR2FeatureSetT* NFTDataLoader::ReadFeatureSet(boost::filesystem::path path)
{
  AR2FeatureSetT *featureSet = NULL;
  FILE *fp = NULL;
  int i, j, l3;
 
  path.replace_extension(".fset");
  if ((fp = fopen(path.generic_string().c_str(), "rb")) == NULL)
  {
    goto done;
  }
 
  arMalloc(featureSet, AR2FeatureSetT, 1);
  if (fread(&(featureSet->num), sizeof(featureSet->num), 1, fp) != 1)
  {
    goto bail0;
  }
 
  arMalloc(featureSet->list, AR2FeaturePointsT, featureSet->num);
  for (i = 0; i < featureSet->num; i++)
  {
    if (fread(&(featureSet->list[i].scale), sizeof(featureSet->list[i].scale), 1, fp) != 1)
    {
      goto bail1;
    }
    if (fread(&(featureSet->list[i].maxdpi), sizeof(featureSet->list[i].maxdpi), 1, fp) != 1)
    {
      goto bail1;
    }
    if (fread(&(featureSet->list[i].mindpi), sizeof(featureSet->list[i].mindpi), 1, fp) != 1)
    {
      goto bail1;
    }
    if (fread(&(featureSet->list[i].num), sizeof(featureSet->list[i].num), 1, fp) != 1)
    {
      goto bail1;
    }
 
    arMalloc(featureSet->list[i].coord, AR2FeatureCoordT, featureSet->list[i].num);
    for (j = 0; j < featureSet->list[i].num; j++)
    {
      if (fread(&(featureSet->list[i].coord[j].x), sizeof(featureSet->list[i].coord[j].x), 1, fp) != 1)
      {
        goto bail1;
      }
      if (fread(&(featureSet->list[i].coord[j].y), sizeof(featureSet->list[i].coord[j].y), 1, fp) != 1)
      {
        goto bail1;
      }
      if (fread(&(featureSet->list[i].coord[j].mx), sizeof(featureSet->list[i].coord[j].mx), 1, fp) != 1)
      {
        goto bail1;
      }
      if (fread(&(featureSet->list[i].coord[j].my), sizeof(featureSet->list[i].coord[j].my), 1, fp) != 1)
      {
        goto bail1;
      }
      if (fread(&(featureSet->list[i].coord[j].maxSim), sizeof(featureSet->list[i].coord[j].maxSim), 1, fp) != 1)
      {
        goto bail1;
      }
    }
  }
 
  goto done;
 
bail1:
  for (l3 = 0; l3 < i; l3++) {
    free(featureSet->list[l3].coord);
  }
  free(featureSet->list);
bail0:
  free(featureSet);
  featureSet = NULL;
 
done:
  fclose(fp);
  return featureSet;
}
 
AR2ImageT* NFTDataLoader::GetImageLayer(AR2ImageT* src, float dpi)
{
  AR2ImageT   *dst;
  ARUint8     *p1, *p2;
  int          wx, wy;
  int          sx, sy, ex, ey;
  int          ii, jj, iii, jjj;
  int          co, value;
 
  wx = (int)lroundf(src->xsize * dpi / src->dpi);
  wy = (int)lroundf(src->ysize * dpi / src->dpi);
 
  arMalloc(dst, AR2ImageT, 1);
  dst->xsize = wx;
  dst->ysize = wy;
  dst->dpi = dpi;
  arMalloc(dst->imgBW, ARUint8, wx*wy);
  p2 = dst->imgBW;
 
  for (jj = 0; jj < wy; jj++)
  {
    sy = (int)lroundf(jj * src->dpi / dpi);
    ey = (int)lroundf((jj + 1) * src->dpi / dpi) - 1;
    if (ey >= src->ysize) ey = src->ysize - 1;
    for (ii = 0; ii < wx; ii++)
    {
      sx = (int)lroundf(ii * src->dpi / dpi);
      ex = (int)lroundf((ii + 1) * src->dpi / dpi) - 1;
      if (ex >= src->xsize) ex = src->xsize - 1;
 
      co = value = 0;
      for (jjj = sy; jjj <= ey; jjj++)
      {
        p1 = &(src->imgBW[jjj*src->xsize + sx]);
        for (iii = sx; iii <= ex; iii++)
        {
          value += *(p1++);
          co++;
        }
      }
      *(p2++) = (co == 0) ? 0 : value / co;
    }
  }
 
  return dst;
}
 
ArTkMarker::ArTkMarker(std::string filePath, unsigned int nid, float size, unsigned int maxFeatures) : ArMarker(nid, size, ArMarker::AR_ARTK_FFT_MARKER)
{
  m_refDataSet = 0;
  m_surfaceSet = 0;
  m_featureSet = 0;
  m_isTrained = false;
 
  m_file = filePath;
  m_maxFeatures = maxFeatures;
 
  if (!m_file.empty())
  {
    boost::filesystem::path bpath = filePath;
    if (bpath.extension() == ".fset" || bpath.extension() == ".fset3")
    {
      bpath.replace_extension("");
      NFTDataLoader nftLoader;
      //kpmLoadRefDataSet(bpath.generic_string().c_str(), "fset3", &m_refDataSet);
      //m_surfaceSet = ar2ReadSurfaceSet(bpath.generic_string().c_str(), "fset", NULL);
 
      if (nftLoader.LoadData(bpath, &m_refDataSet, &m_surfaceSet) && m_refDataSet && m_surfaceSet)
      {
        m_imageSize.width = m_refDataSet->pageInfo[0].imageInfo[0].width;
        m_imageSize.height = m_refDataSet->pageInfo[0].imageInfo[0].height;
        m_pcorners.clear();
        m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
        m_pcorners.push_back(cv::Point2f(static_cast<float>(m_imageSize.width), 0.0f));
        m_pcorners.push_back(cv::Point2f(static_cast<float>(m_imageSize.width), static_cast<float>(m_imageSize.height)));
        m_pcorners.push_back(cv::Point2f(0.0f, static_cast<float>(m_imageSize.height)));
 
        m_isTrained = true;
      }
    }
    else
    {
#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();
  }
 
ArTkMarker::ArTkMarker(cv::Mat tpl, unsigned int nid, float size, unsigned int maxFeatures) : ArMarker(nid, size, ArMarker::AR_ARTK_FFT_MARKER)
{
  m_refDataSet = 0;
  m_surfaceSet = 0;
  m_featureSet = 0;
  m_isTrained = false;
 
  m_file = "";
  m_maxFeatures = maxFeatures;
 
  cv::cvtColor(tpl, m_image, cv::COLOR_RGB2GRAY);
 
  CommonConstructor();
}
 
void ArTkMarker::SetTrackedImage(cv::Mat tpl)
{
  if (!tpl.empty())
  {
    SetTrainned(false);
    SetTracked(false);
 
    m_cancelTrainning = true;
    if (m_trainingThread.joinable())
      m_trainingThread.join();
 
    tpl.copyTo(m_image);
    m_cancelTrainning = false;
 
    //rotate marker 90°
    /*if (m_image.cols > m_image.rows)
    {
      cv::Mat rot;
      cv::transpose(m_image, rot);
      cv::flip(rot, m_image, 1);
    }*/
 
    int msize = MIN_TRACKED_MARKER_SIZE;
    if (std::max(m_image.rows, m_image.cols) < msize)
    {
      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);
    }
 
    m_imageSize.width = m_image.cols;
    m_imageSize.height = m_image.rows;
 
    m_pcorners.clear();
    m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
    m_pcorners.push_back(cv::Point2f(static_cast<float>(m_imageSize.width), 0.0f));
    m_pcorners.push_back(cv::Point2f(static_cast<float>(m_imageSize.width), static_cast<float>(m_imageSize.height)));
    m_pcorners.push_back(cv::Point2f(0.0f, static_cast<float>(m_imageSize.height)));
    m_trainingThread = boost::thread(&ArTkMarker::initTrackingPicture, this);
  }
}
 
void ArTkMarker::CommonConstructor()
{
  mNumGoodTracking = 0;
  m_pageNum = 0;
#ifdef _WIN32
  m_ftmi = arFilterTransMatInit(30.0, 28.0);//arFilterTransMatInit(AR_FILTER_TRANS_MAT_SAMPLE_RATE_DEFAULT, AR_FILTER_TRANS_MAT_CUTOFF_FREQ_DEFAULT);
#else
  m_ftmi = arFilterTransMatInit(30.0, 20.0);//arFilterTransMatInit(AR_FILTER_TRANS_MAT_SAMPLE_RATE_DEFAULT, AR_FILTER_TRANS_MAT_CUTOFF_FREQ_DEFAULT);
#endif
  m_registerNextFrame = false;
  m_isTracked = false;
  m_cancelTrainning = false;
 
  //m_CornerSmoothers.Init(0.2f, 0.2f, 0.2f, 0.20f, 0.15f);
 
  if (!m_isTrained)
  {
    m_refDataSet = 0;
    m_surfaceSet = 0;
    m_featureSet = 0;
 
    m_pcorners.clear();
    m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
    m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
    m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
    m_pcorners.push_back(cv::Point2f(0.0f, 0.0f));
 
    SetTrackedImage(m_image);
  }
  else
  {
    ArManager::GetInstance()->SetNFTdetectorDirty();
  }
}
 
ArTkMarker::~ArTkMarker()
{
  m_cancelTrainning = true;
  if (m_trainingThread.joinable())
    m_trainingThread.join();
 
  std::vector<int> empty(2);
  empty[0] = 0; empty[1] = 0;
  m_image = cv::Mat(empty);
  m_pcorners.clear();
 
  if (m_refDataSet)
    kpmDeleteRefDataSet(&m_refDataSet);
 
  if (m_surfaceSet)
    ar2FreeSurfaceSet(&m_surfaceSet);
 
  if (m_ftmi)
    arFilterTransMatFinal(m_ftmi);
 
  ArManager::GetInstance()->SetNFTdetectorDirty();
}
 
bool ArTkMarker::SaveData(std::string path)
{
  if (!m_isTrained)
    return false;
 
  boost::system::error_code ec;
  boost::filesystem::path bpath = path;
  bpath.replace_extension(".fset");
  if (boost::filesystem::exists(bpath))
    boost::filesystem::remove(bpath, ec);
 
  bpath.replace_extension(".fset3");
  if (boost::filesystem::exists(bpath))
    boost::filesystem::remove(bpath, ec);
 
  bpath.replace_extension(".iset");
  if (boost::filesystem::exists(bpath))
    boost::filesystem::remove(bpath, ec);
 
  bpath.replace_extension("");
 
  if (m_surfaceSet && m_surfaceSet->surface[0].imageSet)
    ar2WriteImageSet((char*)bpath.generic_string().c_str(), m_surfaceSet->surface[0].imageSet);
  else
    return false;
 
  if (m_featureSet)
    ar2SaveFeatureSet((char*)bpath.generic_string().c_str(), "fset", m_featureSet);
  else
    return false;
 
  if (m_refDataSet)
    kpmSaveRefDataSet(bpath.generic_string().c_str(), "fset3", m_refDataSet);
  else
    return false;
 
  return true;
}
 
bool ArTkMarker::LoadData(std::string filename)
{
  return false;
}
 
KpmRefDataSet* ArTkMarker::GetDataSet()
{
  if (!IsTrained())
    return NULL;
 
  boost::unique_lock<boost::recursive_mutex> l(m_trainningMutex);
  //clone dataset to avoid destruction from detector
  KpmRefDataSet* refDataSet = NULL;
  arMalloc(refDataSet, KpmRefDataSet, 1);
 
  refDataSet->pageNum = 1;
  refDataSet->num = m_refDataSet->num;
  arMalloc(refDataSet->pageInfo, KpmPageInfo, 1);
  arMalloc(refDataSet->refPoint, KpmRefData, refDataSet->num);
 
  refDataSet->pageInfo[0].pageNo = m_refDataSet->pageInfo[0].pageNo;
  refDataSet->pageInfo[0].imageNum = m_refDataSet->pageInfo[0].imageNum;
  arMalloc(refDataSet->pageInfo[0].imageInfo, KpmImageInfo, m_refDataSet->pageInfo[0].imageNum);
 
  for (int i = 0; i < refDataSet->pageInfo[0].imageNum; i++)
  {
    refDataSet->pageInfo[0].imageInfo[i].imageNo = m_refDataSet->pageInfo[0].imageInfo[i].imageNo;
    refDataSet->pageInfo[0].imageInfo[i].height = m_refDataSet->pageInfo[0].imageInfo[i].height;
    refDataSet->pageInfo[0].imageInfo[i].width = m_refDataSet->pageInfo[0].imageInfo[i].width;
  }
 
  for (int i = 0; i < refDataSet->num; i++)
  {
    refDataSet->refPoint[i].coord2D.x = m_refDataSet->refPoint[i].coord2D.x;
    refDataSet->refPoint[i].coord2D.y = m_refDataSet->refPoint[i].coord2D.y;
    refDataSet->refPoint[i].coord3D.x = m_refDataSet->refPoint[i].coord3D.x;
    refDataSet->refPoint[i].coord3D.y = m_refDataSet->refPoint[i].coord3D.y;
    refDataSet->refPoint[i].featureVec = m_refDataSet->refPoint[i].featureVec;
    refDataSet->refPoint[i].pageNo = m_refDataSet->refPoint[i].pageNo;
    refDataSet->refPoint[i].refImageNo = m_refDataSet->refPoint[i].refImageNo;
  }
 
  //kpmMergeRefDataSet(&refDataSet, &m_refDataSet);
 
  //it's called on detector reset
  SetVisible(false);
  SetTracked(false);
  return refDataSet;
}
 
int ArTkMarker::GetPageNum()
{
  boost::unique_lock<boost::recursive_mutex> l(m_dataMutex);
  return m_pageNum;
}
 
void ArTkMarker::SetPageNum(int page)
{
  boost::unique_lock<boost::recursive_mutex> l(m_dataMutex);
  m_pageNum = page;
}
 
void ArTkMarker::SetTracked(bool state)
{
  boost::unique_lock<boost::recursive_mutex> l(m_dataMutex);
  m_isTracked = state;
}
 
void ArTkMarker::SetTrainned(bool state)
{
  boost::unique_lock<boost::recursive_mutex> l(m_dataMutex);
  m_isTrained = state;
}
 
bool ArTkMarker::IsTracked()
{
  boost::unique_lock<boost::recursive_mutex> l(m_dataMutex);
  return m_isTracked;
}
 
bool ArTkMarker::IsTrained()
{
  boost::unique_lock<boost::recursive_mutex> l(m_dataMutex);
  return m_isTrained;
}
 
bool ArTkMarker::initTrackingPicture()
{
  boost::unique_lock<boost::recursive_mutex> l(m_trainningMutex);
 
  if (m_refDataSet)
  {
    kpmDeleteRefDataSet(&m_refDataSet);
    m_refDataSet = 0;
  }
 
  if (m_surfaceSet)
  {
    ar2FreeSurfaceSet(&m_surfaceSet);
    m_surfaceSet = 0;
    m_featureSet = 0;
  }
 
  // Determine minimum allowable DPI, truncated to 3 decimal places.
  //max dpi is set to make the marker to 1m size
  float dpiMax = std::max(m_imageSize.width, m_imageSize.height) / 3.93701f;
  float dpiMin = truncf((28.0f / (float)(std::min(m_imageSize.width, m_imageSize.height))) * dpiMax * 1000.0f) / 1000.0f;
  float dpiWork = 0.0f;
  int dpi_num = 0;
  int i, j, k;
 
  if (dpiMin == dpiMax)
  {
    dpi_num = 1;
  }
  else
  {
    dpiWork = dpiMax;
    for (i = 1;; i++)
    {
      dpiWork *= 0.75f;
      if (dpiWork <= dpiMin*0.95f)
      {
        break;
      }
    }
    dpi_num = i + 1;
  }
 
  //limit levels
  if (std::max(m_imageSize.width, m_imageSize.height) <= MIN_TRACKED_MARKER_SIZE)
    dpi_num = std::min(dpi_num, 6); // for live trainning
  else if (std::max(m_imageSize.width, m_imageSize.height) >= 800)
    dpi_num = std::min(dpi_num, 16); // for big pictures
  else if (std::max(m_imageSize.width, m_imageSize.height) >= 1024)
    dpi_num = std::min(dpi_num, 8); // for very big pictures
 
  float* dpi_list = 0;
  arMalloc(dpi_list, float, dpi_num);
 
  dpiWork = dpiMax;
  for (i = 0; i < dpi_num; i++)
  {
    dpi_list[i] = dpiWork; // Lowest value goes at tail of array, highest at head.
    dpiWork *= 0.75f;
 
    if (dpiWork <= dpiMin*0.95f)
      dpiWork = dpiMin;
  }
 
  //check cancel
  if (m_cancelTrainning)
    return false;
 
  AR2ImageSetT *imageSet = ar2GenImageSet(m_image.data, m_image.cols, m_image.rows, 1, dpiMax, dpi_list, dpi_num);
 
  // don't need anymore
  free(dpi_list);
 
  AR2FeatureMapT *featureMap = NULL;
 
  arMalloc(m_featureSet, AR2FeatureSetT, 1);                      // A featureSet with a single image,
  arMalloc(m_featureSet->list, AR2FeaturePointsT, imageSet->num); // and with 'num' scale levels of this image.
 
  m_featureSet->num = imageSet->num;
 
  int numFeatures;
  float scale1 = 0.0f;
  float scale2 = 2.5f;
 
  // Generating FeatureList
  for (i = 0; i < imageSet->num; i++)
  {
    //check cancel
    if (m_cancelTrainning)
      return false;
 
    featureMap = ar2GenFeatureMap(imageSet->scale[i],
      AR2_DEFAULT_TS1*AR2_TEMP_SCALE, AR2_DEFAULT_TS2*AR2_TEMP_SCALE,
      AR2_DEFAULT_GEN_FEATURE_MAP_SEARCH_SIZE1, AR2_DEFAULT_GEN_FEATURE_MAP_SEARCH_SIZE2,
      AR2_DEFAULT_MAX_SIM_THRESH2, AR2_DEFAULT_SD_THRESH2);
 
    if (featureMap == NULL)
    {
      ar2FreeImageSet(&imageSet);
      ar2FreeFeatureSet(&m_featureSet);
      m_featureSet = 0;
 
      //callback for scol object
      OBJpostEvent(AR_MARKER_TRAINNED_CB, SCOL_PTR this, 0);
      return false;
    }
 
    //check cancel
    if (m_cancelTrainning)
      return false;
 
    m_featureSet->list[i].coord = ar2SelectFeature2(imageSet->scale[i], featureMap,
      AR2_DEFAULT_TS1*AR2_TEMP_SCALE, AR2_DEFAULT_TS2*AR2_TEMP_SCALE, AR2_DEFAULT_GEN_FEATURE_MAP_SEARCH_SIZE2,
      AR2_DEFAULT_OCCUPANCY_SIZE * 2 / 3,
      AR2_DEFAULT_MAX_SIM_THRESH_L3, AR2_DEFAULT_MIN_SIM_THRESH_L3, AR2_DEFAULT_SD_THRESH_L3, &numFeatures);
 
    if (m_featureSet->list[i].coord == NULL)
      numFeatures = 0;
 
    m_featureSet->list[i].num = numFeatures;
    m_featureSet->list[i].scale = i;
 
    scale1 = 0.0f;
    for (j = 0; j < imageSet->num; j++)
    {
      if (imageSet->scale[j]->dpi < imageSet->scale[i]->dpi)
      {
        if (imageSet->scale[j]->dpi > scale1)
          scale1 = imageSet->scale[j]->dpi;
      }
    }
 
    if (scale1 == 0.0f)
    {
      m_featureSet->list[i].mindpi = imageSet->scale[i]->dpi * 0.5f;
    }
    else
    {
      m_featureSet->list[i].mindpi = scale1;
    }
 
    scale1 = 0.0f;
    for (j = 0; j < imageSet->num; j++)
    {
      if (imageSet->scale[j]->dpi > imageSet->scale[i]->dpi)
      {
        if (scale1 == 0.0f || imageSet->scale[j]->dpi < scale1)
          scale1 = imageSet->scale[j]->dpi;
      }
    }
 
    if (scale1 == 0.0f)
    {
      m_featureSet->list[i].maxdpi = imageSet->scale[i]->dpi * 2.0f;
    }
    else
    {
      scale2 = imageSet->scale[i]->dpi;
      m_featureSet->list[i].maxdpi = scale2*0.8f + scale1*0.2f;
    }
 
    ar2FreeFeatureMap(featureMap);
  }
 
  //check cancel
  if (m_cancelTrainning)
    return false;
 
  //surface set
  arMalloc(m_surfaceSet, AR2SurfaceSetT, 1);
  m_surfaceSet->num = 1;
  m_surfaceSet->contNum = 0;
  arMalloc(m_surfaceSet->surface, AR2SurfaceT, m_surfaceSet->num);
  m_surfaceSet->surface[0].imageSet = imageSet;
  m_surfaceSet->surface[0].featureSet = m_featureSet;
  m_surfaceSet->surface[0].markerSet = NULL;
  m_surfaceSet->surface[0].jpegName = NULL;
 
  for (j = 0; j < 3; j++)
  {
    for (k = 0; k < 4; k++)
    {
      m_surfaceSet->surface[0].trans[j][k] = (j == k) ? 1.0f : 0.0f;
    }
  }
 
  arUtilMatInvf((const float(*)[4])m_surfaceSet->surface[0].trans, m_surfaceSet->surface[0].itrans);
 
  int procMode = KpmProcFullSize;
  int maxFeatureNum;
  int featureDensity = m_maxFeatures;
 
  for (i = 0; i < imageSet->num; i++)
  {
    //check cancel
    if (m_cancelTrainning)
      return false;
 
    maxFeatureNum = featureDensity * imageSet->scale[i]->xsize * imageSet->scale[i]->ysize / (480 * 360);
 
    if (!maxFeatureNum || kpmAddRefDataSet(imageSet->scale[i]->imgBW, imageSet->scale[i]->xsize, imageSet->scale[i]->ysize,
      imageSet->scale[i]->dpi,
      procMode, KpmCompNull, maxFeatureNum, 1, i, &m_refDataSet) < 0)
    {
      // Page number set to 1 by default.
      kpmDeleteRefDataSet(&m_refDataSet);
      m_refDataSet = 0;
      ar2FreeSurfaceSet(&m_surfaceSet);
      m_surfaceSet = 0;
      m_featureSet = 0;
      //callback for scol object
      OBJpostEvent(AR_MARKER_TRAINNED_CB, SCOL_PTR this, 0);
      return false;
    }
  }
 
  //ar2FreeImageSet(&imageSet);
 
  //ar2SaveFeatureSet
  //kpmSaveRefDataSet
 
  ArManager::GetInstance()->SetNFTdetectorDirty();
  SetTrainned(true);
 
  //callback for scol object
  OBJpostEvent(AR_MARKER_TRAINNED_CB, SCOL_PTR this, 1);
  return true;
}
 
void ArTkMarker::RegisterNextFrame(cv::Point point, cv::Size size)
{
  m_cropPos = point;
  m_cropSize = size;
  m_registerNextFrame = true;
}
 
bool ArTkMarker::GetWarpedMarker(cv::Mat &image)
{
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
  m_warped.copyTo(image);
 
  if (image.empty())
    return false;
  else
    return true;
}
 
void ArTkMarker::WarpMarkerImage(cv::Mat color)
{
  cv::Mat HMatrix = cv::findHomography(m_pcorners, *this);
  // get warped detected marker
  boost::unique_lock<boost::recursive_mutex> l(m_mutex);
  cv::warpPerspective(color, m_warped, HMatrix, cv::Size(m_imageSize.width, m_imageSize.height), cv::WARP_INVERSE_MAP | cv::INTER_CUBIC);
}
 
void ArTkMarker::ComputeMatrix(ArCameraParam &camParams, bool reverse)
{
  float halfSizeX = 0.0f;
  float halfSizeY = 0.0f;
  if (m_imageSize.height >= m_imageSize.width)
  {
    halfSizeY = m_size / 2.0f;
    halfSizeX = ((m_imageSize.width * m_size) / m_imageSize.height) / 2.0f;
  }
  else
  {
    halfSizeY = ((m_imageSize.height * m_size) / m_imageSize.width) / 2.0f;
    halfSizeX = m_size / 2.0f;
  }
 
  /*
  m_CornerSmoothers.Update(filterVals);
  filterVals = m_CornerSmoothers.GetFilteredPoints();
 
  int p = 0;
  for (j = 0; j < 3; j++)
  for (i = 0; i < 4; i++)
  {
  mTackingPos[j][i] = filterVals[p];
  p++;
  }
  */
 
  //filter
  arFilterTransMat(m_ftmi, mTackingPos, false);
 
  double modelview_matrix[16];
 
  //mul matrix for 90° rotation
  cv::Mat R(4, 4, CV_32F);
  R.at<float>(0, 0) = mTackingPos[0][0]; // R1C1
  R.at<float>(0, 1) = mTackingPos[0][1]; // R1C2
  R.at<float>(0, 2) = mTackingPos[0][2];
  R.at<float>(0, 3) = mTackingPos[0][3];
  R.at<float>(1, 0) = mTackingPos[1][0]; // R2
  R.at<float>(1, 1) = mTackingPos[1][1];
  R.at<float>(1, 2) = mTackingPos[1][2];
  R.at<float>(1, 3) = mTackingPos[1][3];
  R.at<float>(2, 0) = mTackingPos[2][0]; 
  R.at<float>(2, 1) = mTackingPos[2][1];
  R.at<float>(2, 2) = mTackingPos[2][2];
  R.at<float>(2, 3) = mTackingPos[2][3];
 
  // create a rotation matrix for x axis
  cv::Mat RX = cv::Mat::eye(4, 4, CV_32F);
  float angleRad = M_PI / 2;
  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);
 
  // now multiply
  R = R * RX;
 
  modelview_matrix[0 + 0 * 4] = R.at<float>(0, 0); // R1C1
  modelview_matrix[0 + 1 * 4] = R.at<float>(0, 1); // R1C2
  modelview_matrix[0 + 2 * 4] = R.at<float>(0, 2);
  modelview_matrix[0 + 3 * 4] = R.at<float>(0, 3);
  modelview_matrix[1 + 0 * 4] = R.at<float>(1, 0); // R2
  modelview_matrix[1 + 1 * 4] = R.at<float>(1, 1);
  modelview_matrix[1 + 2 * 4] = R.at<float>(1, 2);
  modelview_matrix[1 + 3 * 4] = R.at<float>(1, 3);
  modelview_matrix[2 + 0 * 4] = -R.at<float>(2, 0); // R3
  modelview_matrix[2 + 1 * 4] = -R.at<float>(2, 1);
  modelview_matrix[2 + 2 * 4] = -R.at<float>(2, 2);
  modelview_matrix[2 + 3 * 4] = -R.at<float>(2, 3);
  modelview_matrix[3 + 0 * 4] = 0.0;
  modelview_matrix[3 + 1 * 4] = 0.0;
  modelview_matrix[3 + 2 * 4] = 0.0;
  modelview_matrix[3 + 3 * 4] = 1.0;
 
  if (m_size != 0.0)
  {
    // convert from marker in 120DPI to millimeter
    double f = m_size * 0.01;
    modelview_matrix[12] *= f;
    modelview_matrix[13] *= f;
    modelview_matrix[14] *= f;
 
    // place the position on the center of the object
    cv::Point3d p(halfSizeX, 0.0, -halfSizeY);
    cv::Matx33d Rt = cv::Matx33d::eye();
 
    Rt(0, 0) = modelview_matrix[0 + 0 * 4];
    Rt(0, 1) = modelview_matrix[0 + 1 * 4];
    Rt(0, 2) = modelview_matrix[0 + 2 * 4];
    Rt(1, 0) = modelview_matrix[1 + 0 * 4];
    Rt(1, 1) = modelview_matrix[1 + 1 * 4];
    Rt(1, 2) = modelview_matrix[1 + 2 * 4];
    Rt(2, 0) = modelview_matrix[2 + 0 * 4];
    Rt(2, 1) = modelview_matrix[2 + 1 * 4];
    Rt(2, 2) = modelview_matrix[2 + 2 * 4];
 
    cv::Point3d roff = Rt * p;
    modelview_matrix[12] += roff.x;
    modelview_matrix[13] += roff.y;
    modelview_matrix[14] += roff.z;
 
    cv::Mat rot(3, 3, CV_32FC1);
    rot.at<float>(0, 0) = mTackingPos[0][0];
    rot.at<float>(0, 1) = mTackingPos[0][1];
    rot.at<float>(0, 2) = mTackingPos[0][2];
    rot.at<float>(1, 0) = mTackingPos[1][0];
    rot.at<float>(1, 1) = mTackingPos[1][1];
    rot.at<float>(1, 2) = mTackingPos[1][2];
    rot.at<float>(2, 0) = mTackingPos[2][0];
    rot.at<float>(2, 1) = mTackingPos[2][1];
    rot.at<float>(2, 2) = mTackingPos[2][2];
 
    cv::Rodrigues(rot, Rvec);
 
    Tvec.at<float>(0, 0) = modelview_matrix[12];
    Tvec.at<float>(1, 0) = modelview_matrix[13];
    Tvec.at<float>(2, 0) = -modelview_matrix[14];
  }
 
  std::vector<cv::Point3d> corner3d;
  corner3d.push_back(cv::Point3d(-halfSizeX, halfSizeY, 0.0));
  corner3d.push_back(cv::Point3d(halfSizeX, halfSizeY, 0.0));
  corner3d.push_back(cv::Point3d(halfSizeX, -halfSizeY, 0.0));
  corner3d.push_back(cv::Point3d(-halfSizeX, -halfSizeY, 0.0));
 
  std::vector<cv::Point2d> projectedPoints;
  cv::projectPoints(corner3d, Rvec, Tvec, camParams.camParam.CameraMatrix, camParams.camParam.Distorsion, projectedPoints);
 
  for (int j = 0; j < projectedPoints.size(); j++)
  {
    (*this).at(j).x = (float)projectedPoints[j].x;
    (*this).at(j).y = (float)projectedPoints[j].y;
  }
 
  //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.;
 
  SetPixelPosition(pixelPosition);
 
  Vector3 offset = camParams.GetCameraOffset();
 
  SetPosition(Vector3(static_cast<float>(reverse ? -modelview_matrix[12] : modelview_matrix[12]) + offset.x, static_cast<float>(modelview_matrix[13]) + offset.y, static_cast<float>(modelview_matrix[14]) + offset.z));
  SetOrientation(BtQuaternion::FromRotationMatrix(modelview_matrix, reverse));
}
 
void ArTkMarker::StartTracking(cv::Mat color, const float camPose[3][4], ArCameraParam &camParams, const bool &reverse)
{
  std::vector<double> filterVals;
  int i, j;
  for (j = 0; j < 3; j++)
    for (i = 0; i < 4; i++)
    {
      mCamPose[j][i] = camPose[j][i];
      mTackingPos[j][i] = camPose[j][i];
      //filterVals.push_back(camPose[j][i]);
    }
 
  mNumGoodTracking = 0;
  ar2SetInitTrans(m_surfaceSet, mCamPose);
  //m_CornerSmoothers.Reset();
  //m_CornerSmoothers.Update(filterVals);
  arFilterTransMat(m_ftmi, mTackingPos, true);
  ComputeMatrix(camParams, reverse);
  WarpMarkerImage(color);
 
  if (IsTrained() && m_trainingThread.joinable())
    m_trainingThread.join();
  SetTracked(true);
}
 
bool ArTkMarker::RegisterCurrentFrame(const cv::Mat &frame, cv::Size camsize, float tscale)
{
  if (m_registerNextFrame && camsize.width != 0 && camsize.height != 0 && m_cropSize.width > 1 && m_cropSize.height > 1)
  {
    m_registerNextFrame = false;
    //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 * tscale * scale.x);
    m_cropPos.y = (int)((float)m_cropPos.y * tscale * scale.y);
    m_cropSize.width = (int)((float)m_cropSize.width * tscale * scale.x);
    m_cropSize.height = (int)((float)m_cropSize.height * tscale * scale.x);
 
    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
 
      int msize = MIN_TRACKED_MARKER_SIZE;
      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::resize(crop, crop, nsize, 0, 0, cv::INTER_CUBIC);
 
      SetTrackedImage(crop);
    }
    catch (std::exception &)
    {
      //could not set current frame
      MMechostr(MSKDEBUG, "detectNFT : failed to set the current frame as marker.\n");
      return false;
    }
    SetVisible(false);
    return true;
  }
  return false;
}
 
bool ArTkMarker::TrackFrame(AR2HandleT* ar2handle, const cv::Mat &color, const cv::Mat &frame, ArCameraParam &camParams, const bool &reverse)
{
  if (IsTracked())
  {
    float err = 0;
    if (ar2Tracking(ar2handle, m_surfaceSet, frame.data, mCamPose, &err) < 0)
    {
      SetTracked(false);
      SetVisible(false);
      return false;
    }
    else
    {
      //std::vector<double> filterVals;
      int i, j;
      for (j = 0; j < 3; j++)
        for (i = 0; i < 4; i++)
        {
          mTackingPos[j][i] = mCamPose[j][i];
          //filterVals.push_back(mCamPose[j][i]);
        }
 
      ComputeMatrix(camParams, reverse);
 
 
      //marker visibility is stable
      if (mNumGoodTracking < 4)
        mNumGoodTracking++;
      else
      {
        SetVisible(true);
        //WarpMarkerImage(color);
      };
 
      return true;
    }
  }
  return false;
}