ArTkDetector.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 "ArTkDetector.h"
 
#include <vector>
 
ArTkDetector::ArTkDetector() : btopenutils::Thread()
{
  mIsDirty = false;
  needUpdate = false;
  mDetectionInProgress = false;
  mKpmHandle = 0;
  mAr2Handle = 0;
  mArCamParam = 0;
  mNumSkip = 0;
  mScale = 1.0f;
 
  // Run thread
  StartThreading(boost::bind(&ArTkDetector::GoThread, this));
}
 
ArTkDetector::~ArTkDetector()
{
  StopThreading();
 
  if (mKpmHandle)
    kpmDeleteHandle(&mKpmHandle);
 
  if (mAr2Handle)
    ar2DeleteHandle(&mAr2Handle);
 
  if (mArCamParam)
    arParamLTFree(&mArCamParam);
}
 
void ArTkDetector::AddMarker(ArMarker* marker)
{
  boost::unique_lock<boost::recursive_mutex> lu(mUpdateMutex);
 
  ArTkMarker* nftmarker = static_cast<ArTkMarker*>(marker);
 
  mMarkerList.insert(nftmarker);
 
  lu.unlock();
}
 
void ArTkDetector::RemoveMarker(ArMarker* marker)
{
  boost::unique_lock<boost::recursive_mutex> lu(mUpdateMutex);
 
  std::set<ArTkMarker*>::iterator imarker = mMarkerList.find(static_cast<ArTkMarker*> (marker));
  if (imarker != mMarkerList.end())
    mMarkerList.erase(imarker);
 
  lu.unlock();
}
 
void ArTkDetector::SetDirty()
{
  mIsDirty = true;
}
 
void ArTkDetector::BuildCameraParam(ArCameraParam &camparam)
{
  if (mArCamParam)
    arParamLTFree(&mArCamParam);
 
  mScale = 1.0f;
 
  int maxsize = std::max(camparam.camParam.CamSize.width, camparam.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
  */
 
  //construct ARtk camParam
  ARParam cparam;
  cparam.xsize = (int)((float)camparam.camParam.CamSize.width * mScale);
  cparam.ysize = (int)((float)camparam.camParam.CamSize.height * mScale);
 
  cparam.mat[0][0] = camparam.camParam.CameraMatrix.at<float>(0, 0) * mScale;
  cparam.mat[0][1] = camparam.camParam.CameraMatrix.at<float>(0, 1);
  cparam.mat[0][2] = camparam.camParam.CameraMatrix.at<float>(0, 2) * mScale;
  cparam.mat[0][3] = 0.0;
  cparam.mat[1][0] = camparam.camParam.CameraMatrix.at<float>(1, 0);
  cparam.mat[1][1] = camparam.camParam.CameraMatrix.at<float>(1, 1) * mScale;
  cparam.mat[1][2] = camparam.camParam.CameraMatrix.at<float>(1, 2) * mScale;
  cparam.mat[1][3] = 0.0;
  cparam.mat[2][0] = camparam.camParam.CameraMatrix.at<float>(2, 0);
  cparam.mat[2][1] = camparam.camParam.CameraMatrix.at<float>(2, 1);
  cparam.mat[2][2] = camparam.camParam.CameraMatrix.at<float>(2, 2);
  cparam.mat[2][3] = 0.0;
 
  cparam.dist_function_version = 4;
  cparam.dist_factor[0] = 0.0; // 0.11478076875209808
  cparam.dist_factor[1] = 0.0; // -0.52081894874572754
  cparam.dist_factor[2] = 0.0; // -0.00020698705338872969
  cparam.dist_factor[3] = 0.0; // -0.0040593123994767666
  cparam.dist_factor[4] = cparam.xsize; // 1348.3432617187500
  cparam.dist_factor[5] = cparam.xsize; // 1352.3159179687500
  cparam.dist_factor[6] = cparam.ysize; // 636.59558105468750
  cparam.dist_factor[7] = cparam.ysize; // 507.52099609375000
  cparam.dist_factor[8] = 1.0; // 0.99392257313642507
 
  //arParamLoad("Plugins/btdata/camera_para.dat", 1, &cparam);
  //arParamChangeSize(&cparam, mLastData.gray.cols, mLastData.gray.rows, &cparam);
  mArCamParam = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET);
}
 
void ArTkDetector::InitDetector()
{
  boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
  if (mKpmHandle)
    kpmDeleteHandle(&mKpmHandle);
 
  if (mAr2Handle)
    ar2DeleteHandle(&mAr2Handle);
 
  // KPM init.
  mKpmHandle = kpmCreateHandle(mArCamParam);
  //kpmSetSurfThreadNum(mKpmHandle, 1);
  //kpmSetProcMode(mKpmHandle, KpmProcTwoThirdSize);
  kpmSetDetectedFeatureMax(mKpmHandle, 250);
 
  // AR2 init.
  mAr2Handle = ar2CreateHandle(mArCamParam, AR_PIXEL_FORMAT_MONO, -1);
 
  ar2SetTrackingThresh(mAr2Handle, 5.0);
  ar2SetSimThresh(mAr2Handle, 0.5);
  ar2SetSearchFeatureNum(mAr2Handle, 64);
  ar2SetSearchSize(mAr2Handle, 8);
  ar2SetTemplateSize1(mAr2Handle, 4);
  ar2SetTemplateSize2(mAr2Handle, 6);
 
  // load markers data
  KpmRefDataSet *refDataSet = NULL;
  int pageCount = 0;
 
  std::set<ArTkMarker*>::iterator ifttmarker;
  for (ifttmarker = mMarkerList.begin(); ifttmarker != mMarkerList.end(); ++ifttmarker)
  {
    if (!(*ifttmarker)->IsTrained())
      continue;
 
    //add marker
    KpmRefDataSet *refDataSet2 = (*ifttmarker)->GetDataSet();
    if (refDataSet2 == NULL)
      continue;
 
    (*ifttmarker)->SetPageNum(pageCount);
    kpmChangePageNoOfRefDataSet(refDataSet2, KpmChangePageNoAllPages, pageCount);
    kpmMergeRefDataSet(&refDataSet, &refDataSet2);
    pageCount++;
  }
 
  if (refDataSet != NULL)
  {
    kpmSetRefDataSet(mKpmHandle, refDataSet);
    kpmDeleteRefDataSet(&refDataSet);
  }
 
  ld.unlock();
}
 
void ArTkDetector::NFTDetect()
{
  KpmResult* kpmResult = NULL;
  int kpmResultNum = 0;
  
  {
    boost::mutex::scoped_lock ld(mDetectMutex);
    if (mDetectorFrame.empty() || mDetectorFrame.data == 0)
      return;
 
    kpmSetMatchingSkipPage(mKpmHandle, mSkippage, mNumSkip);
    kpmMatching(mKpmHandle, mDetectorFrame.data);
    kpmGetResult(mKpmHandle, &kpmResult, &kpmResultNum);
  }
 
  boost::unique_lock<boost::recursive_mutex> ld(mUpdateMutex);
  std::set<ArTkMarker*>::iterator imarker;
 
  for (int i = 0; i < kpmResultNum; i++)
  {
    if (kpmResult[i].camPoseF != 0)
      continue;
 
    //found marker ?
    for (imarker = mMarkerList.begin(); imarker != mMarkerList.end(); ++imarker)
    {
      if (!(*imarker)->IsTracked() && (*imarker)->IsTrained() && ((*imarker)->GetPageNum() == kpmResult[i].pageNo))
        (*imarker)->StartTracking(mDetectorColorFrame, kpmResult[i].camPose, mCamparam, mReverse);
    }
  }
  mDetectionInProgress = false;
  ld.unlock();
}
 
void ArTkDetector::GoThread()
{
  boost::thread detectorThread;
  while (IsRunning())
  {
    if (needUpdate)
    {
      ArManager* manager = ArManager::GetInstance();
      manager->GetLastData(mLastData);
 
      needUpdate = false;
      bool needDetect = false;
 
      //cv::blur(mLastData.gray, mLastData.gray, cv::Size(2, 2));
 
      // Detect all markers seen on gray
 
      // camera size changed
      if ((mArCamParam == 0) || (mArCamParam && (((int)((float)mLastData.arCamParam.camParam.CamSize.width * mScale)) != mArCamParam->param.xsize || ((int)((float)mLastData.arCamParam.camParam.CamSize.height * mScale)) != mArCamParam->param.ysize)))
        mIsDirty = true;
 
      // init detector
      if (mIsDirty && !mMarkerList.empty())
      {
        boost::mutex::scoped_lock ld(mDetectMutex);
        mIsDirty = false;
        if (detectorThread.joinable())
          detectorThread.join();
        BuildCameraParam(mLastData.arCamParam);
        InitDetector();
      }
      
      cv::Size nsize;
      nsize.width = (int)((float)mLastData.gray.cols * mScale);
      nsize.height = (int)((float)mLastData.gray.rows * mScale);
 
      if (mScale != 1.0f)
        cv::resize(mLastData.gray, mLastData.gray, nsize, 0, 0, cv::INTER_LINEAR);
 
      cv::equalizeHist(mLastData.gray, mLastData.gray);
 
      boost::unique_lock<boost::recursive_mutex> lu(mUpdateMutex);
      std::set<ArTkMarker*>::iterator imarker;
 
      for (imarker = mMarkerList.begin(); imarker != mMarkerList.end(); ++imarker)
      {
        if (!(*imarker)->IsTracked())
          needDetect = true;
      }
 
      //tracking
      for (imarker = mMarkerList.begin(); imarker != mMarkerList.end(); ++imarker)
      {
        if ((*imarker)->IsTracked())
          (*imarker)->TrackFrame(mAr2Handle, mLastData.image, mLastData.gray, mLastData.arCamParam, mLastData.reversedBitmap);
 
        (*imarker)->RegisterCurrentFrame(mLastData.gray, nsize, mScale);
      }
 
      // build nft detector mask
      if (needDetect && !mDetectionInProgress && !mLastData.image.empty())
      {
        boost::mutex::scoped_lock ld(mDetectMutex);
 
        if (detectorThread.joinable())
          detectorThread.join();
 
        mDetectionInProgress = true;
        mLastData.gray.copyTo(mDetectorFrame);
        mNumSkip = 0;
 
        //mask tracked markers
        unsigned int i;
        for (imarker = mMarkerList.begin(); imarker != mMarkerList.end(); ++imarker)
        {
          if ((*imarker)->IsTracked())
          {
            std::vector<cv::Point> poly;
            for (i = 0; i < (*imarker)->size(); i++)
            {
              cv::Point pt1(static_cast<int>((*imarker)->at(i).x * mScale), static_cast<int>((*imarker)->at(i).y * mScale));
              poly.push_back(pt1);
            }
            cv::fillConvexPoly(mDetectorFrame, poly, cv::Scalar(0));
            mSkippage[mNumSkip] = (*imarker)->GetPageNum();
            mNumSkip++;
          }
        }
 
        mCamparam = mLastData.arCamParam;
        mReverse = mLastData.reversedBitmap;
        //cv::resize(mLastData.image, mDetectorColorFrame, nsize, 0, 0, cv::INTER_CUBIC);
        mLastData.image.copyTo(mDetectorColorFrame);
        detectorThread = boost::thread(&ArTkDetector::NFTDetect, this);
      }
 
      lu.unlock();
    }
    else
    {
      //prevent cpu burn
      boost::this_thread::sleep_for(boost::chrono::milliseconds(1)); //DO not burn too much CPU
    }
  }
 
  if (detectorThread.joinable())
    detectorThread.join();
}