ArManager.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 "ArManager.h"
#include "ArCameraParam.h"
 
#include <vector>
 
// Initialize singleton
ArManager* ArManager::_singleton = 0;
 
ArManager::ArManager()
{
  detector = 0;
  fdetector = 0;
  facedetector = 0;
  tkdetector = 0;
 
#ifdef HAVE_BTSLAM
  slamdetector = 0;
#endif
}
 
ArManager::~ArManager()
{
  SAFE_DELETE(detector);
  SAFE_DELETE(fdetector);
  SAFE_DELETE(facedetector);
  SAFE_DELETE(tkdetector);
 
#ifdef HAVE_BTSLAM
  SAFE_DELETE(slamdetector);
#endif
  //SAFE_DELETE(ndetector);
  
  for (unsigned int i = 0; i < bdetectorList.size(); i ++)
    SAFE_DELETE(bdetectorList[i]);
  
  bdetectorList.clear();
  
        for (MarkerList::iterator imarker = markerList.begin(); imarker != markerList.end(); imarker++)
        {
                ArMarker* marker = *imarker;
                SAFE_DELETE(marker);
        }
  markerList.clear();
}
 
ArManager* ArManager::GetInstance()
{
  if (0 == _singleton)
  {
    _singleton =  new ArManager();
  }
 
  return _singleton;
}
 
void ArManager::Kill()
{
  if (0 != _singleton)
  {
    delete _singleton;
    _singleton = 0;
  }
}
 
ArBlobDetector* ArManager::AddBlobDetector(cv::SimpleBlobDetector::Params params)
{
  ArBlobDetector* detector = new ArBlobDetector(params);
  bdetectorList.push_back(detector);
 
  return detector;
}
 
void ArManager::RemoveBlobDetector(ArBlobDetector* detector)
{
  for (unsigned int i = 0; i < bdetectorList.size(); i++)
  {
    if (bdetectorList[i] == detector)
    {
      SAFE_DELETE(detector);
      bdetectorList.erase(bdetectorList.begin() + i);
      return;
    }
  }
}
 
#ifdef HAVE_BTSLAM
void ArManager::EnableSlamDetector(bool state)
{
  if (state && slamdetector)
    return;
  else if (state)
    slamdetector = new Sslam();
  else if (!state)
    SAFE_DELETE(slamdetector);
}
 
void ArManager::SetSlamDetectorParams(int nbp, int nbimp, int minf, int maxf)
{
  dso::setting_desiredPointDensity = nbp;
  dso::setting_desiredImmatureDensity = nbimp;
  dso::setting_minFrames = minf;
  dso::setting_maxFrames = maxf;
  if (slamdetector)
    slamdetector->Reset();
}
 
Vector3 ArManager::GetSlamCameraPostition()
{
  if (!slamdetector)
    return Vector3();
 
  return slamdetector->GetCameraPosition();
}
 
BtQuaternion ArManager::GetSlamCameraOrientation()
{
  if (!slamdetector)
    return BtQuaternion();
 
  return slamdetector->GetCameraOrientation();
}
 
bool ArManager::IsSlamFound()
{
  if (!slamdetector)
    return false;
 
  return slamdetector->IsFound();
}
 
void ArManager::ResetSlamDetector()
{
  if (!slamdetector)
    return;
 
  return slamdetector->Reset();
}
#endif
 
void ArManager::UpdateMarkers(cv::Mat image, ArCameraParam* arCameraParam, bool reversedBitmap, bool debugDraw)
{
  if (image.cols != arCameraParam->GetBitmapSize().width || image.rows != arCameraParam->GetBitmapSize().height)
    return;
 
#ifdef HAVE_BTSLAM
  if (!image.empty() && (detector || fdetector || tkdetector || slamdetector || facedetector || !bdetectorList.empty()))
#else
  if (!image.empty() && (detector || fdetector || tkdetector || facedetector || !bdetectorList.empty()))
#endif
  {
    //just don't call update when the camera return nil to avoid double detection
    /*
    try
    {
      if (!lastData.frame.empty())
      {
        cv::Mat diff;
        cv::cvtColor(image, diff, cv::COLOR_BGR2GRAY);
        cv::compare(lastData.image, diff, diff, cv::CMP_NE);
     
        if (cv::countNonZero(diff) == 0)
          return;
      }
    }
    catch(std::exception &)
    {
      // camera param
      aruco::CameraParameters camParam = arCameraParam.GetCameraParameter();
      lastData = LASTDATA(image, camParam, reversedBitmap);
      return;
    }*/
 
    // camera param
    {
      //exclusive access (write)
      boost::unique_lock< boost::shared_mutex > lock(updateMutex);
      double timeStamp = std::chrono::duration<double, std::nano>(std::chrono::system_clock::now().time_since_epoch()).count() / 1e9;
      double delta = (lastData.timeStamp == 0.0) ? 0.0 : timeStamp - lastData.timeStamp;
      lastData = LASTDATA(image, arCameraParam, reversedBitmap, timeStamp, delta);
      arCameraParam->ResetCameraImu();
    }
 
    if (debugDraw)
    {
      //read only
      boost::shared_lock< boost::shared_mutex > lockList(listMutex);
      MarkerList::iterator imarker;
      for (imarker=markerList.begin(); imarker!=markerList.end(); ++imarker)
      {
        if((*imarker)->IsVisible())
          (*imarker)->Draw(image);
      }
    }
 
#ifdef HAVE_BTSLAM
    if (slamdetector)
    {
      slamdetector->needUpdate = true;
      if (debugDraw)
        slamdetector->DrawLandmarks(image);
    }
#endif
 
          if (tkdetector)
            tkdetector->needUpdate = true;
 
    if (fdetector)
      fdetector->needUpdate = true;
    
    if(facedetector)
      facedetector->needUpdate = true;
 
    for (unsigned int i = 0; i < bdetectorList.size(); i ++)
     bdetectorList[i]->needUpdate = true;
 
    if (detector)
      detector->needUpdate = true;
  }
}
 
ArMarker* ArManager::AddMarker(int index, float size)
{
  //conditional lock
  boost::upgrade_lock< boost::shared_mutex > lock(listMutex);
  //take the lock exclusive
  boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
 
  if(!detector)
  {
    detector = new ArDetector();
  }
 
        ArMarker* marker = 0;
        if ((index >= 0) || (index < 1024))
        {
    marker = new ArMarker(index, size, ArMarker::AR_ARUCO_MARKER);
                markerList.insert(marker);
        }
        return marker;
}
 
ArMarker* ArManager::AddMarker(std::string path, float size, unsigned int maxFeatures)
{
  //conditional lock
  boost::upgrade_lock< boost::shared_mutex > lock(listMutex);
  //take the lock exclusive
  boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
 
  if (!tkdetector)
  {
    tkdetector = new ArTkDetector();
  }
    
  /*if(!fdetector)
  {
    fdetector = new ArFeaturedDetector();
  }*/
 
        //ArMarker* marker = new ArFeaturedMarker(path, CalcNewID(), size, maxFeatures);
  ArMarker* marker = new ArTkMarker(path, CalcNewID(), size, maxFeatures);
  tkdetector->AddMarker(marker);
  markerList.insert(marker);
 
        return marker;
}
 
ArMarker* ArManager::AddMarker(cv::Mat tpl, float size, unsigned int maxFeatures)
{
  //conditional lock
  boost::upgrade_lock< boost::shared_mutex > lock(listMutex);
  //take the lock exclusive
  boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
/*
  if (!tkdetector)
  {
    tkdetector = new ArTkDetector();
  }*/
  
  if(!fdetector)
  {
    fdetector = new ArFeaturedDetector();
  }
  
        ArMarker* marker = new ArFeaturedMarker(tpl, CalcNewID(), size, maxFeatures);
  markerList.insert(marker);
 
        return marker;
}
 
ArMarker* ArManager::AddFaceMarker(float size)
{
  //conditional lock
  boost::upgrade_lock< boost::shared_mutex > lock(listMutex);
  //take the lock exclusive
  boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
 
  if(!facedetector)
  {
    facedetector = new ArFaceDetector();
  }
 
        ArMarker* marker = new ArFaceMarker(CalcNewID(), size);
  markerList.insert(marker);
 
        return marker;
}
 
void ArManager::RemoveMarker(ArMarker* marker)
{
  //conditional lock
  boost::upgrade_lock< boost::shared_mutex > lock(listMutex);
  
  MarkerList::iterator imarker = markerList.find(marker);
  if (imarker != markerList.end())
  {
    //take the lock exclusive
    boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
 
    if (tkdetector && (*imarker)->GetType() == ArMarker::AR_ARTK_FFT_MARKER)
      tkdetector->RemoveMarker(*imarker);
 
    markerList.erase(imarker);
 
    SAFE_DELETE(marker);
  }
 
  if(markerList.size() == 0)
  {
    SAFE_DELETE(detector);
    SAFE_DELETE(fdetector);
    SAFE_DELETE(facedetector);
    SAFE_DELETE(tkdetector);
  }
}
 
void ArManager::updateFiducialMarkers(std::vector<aruco::Marker> detectedMarkers, LASTDATA lastd)
{
  //read only
  boost::shared_lock< boost::shared_mutex > lockList(listMutex);
 
  std::vector<aruco::Marker*> matchList;
  matchList.resize(1024, 0);
 
  for(unsigned int i = 0; i < detectedMarkers.size(); i++)
  {
    if (detectedMarkers[i].isValid())
      matchList[detectedMarkers[i].id] = &(detectedMarkers[i]);
  }
 
  for (MarkerList::iterator imarker = markerList.begin(); imarker != markerList.end(); ++imarker)
  {
    if ((*imarker)->GetType() == ArMarker::AR_ARUCO_MARKER)
    {
      aruco::Marker* amarker = matchList[(*imarker)->GetID()];
      if(amarker != 0)
      {
        (*imarker)->Update(lastd.arCamParam, *amarker, lastd.reversedBitmap);
      }
      else
      {
        (*imarker)->SetVisible(false);
      }
    }
  }
}
 
unsigned int ArManager::CalcNewID()
{
  unsigned int id = 1024;
        MarkerList::iterator imarker;
        for (imarker = markerList.begin(); imarker != markerList.end(); ++imarker)
        {
                id = std::max(id, (*imarker)->GetID());
        }
  return id + 1;
}
 
void ArManager::GetLastData(LASTDATA &data)
{
  boost::shared_lock< boost::shared_mutex > lockUpdate(updateMutex);
  data = LASTDATA(lastData);
}
 
void ArManager::SetNFTdetectorDirty()
{
  if (tkdetector)
  {
    tkdetector->SetDirty();
  }
}