ArMarker.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 "ArMarker.h"
#include "ArManager.h"
 
ArMarker::ArMarker(unsigned int nid, float markerSize, MarkerType type) : aruco::Marker()
{
  id = nid;
  m_type = type;
  m_needUpdate = false;
        m_visible = false;
        m_size = markerSize;
  m_lastReverse = false;
  m_bUpdating = false;
 
  for (unsigned int i = 0; i < 4; i++)
    (*this).push_back(cv::Point2f(0.0f, 0.0f));
}
 
ArMarker::~ArMarker()
{
}
 
cv::Mat ArMarker::GetMarkerBitmap(int size)
{
  //TODO retrieve the featured picture
  if (m_type != AR_ARUCO_MARKER)
    return cv::Mat();
 
  aruco::Dictionary dict = aruco::Dictionary::loadPredefined(aruco::Dictionary::ARUCO);
        return dict.getMarkerImage_id(id, size / 4, true, false, false);
}
 
void ArMarker::SetPosition(Vector3 pos)
{
        m_pos = pos;
}
 
void ArMarker::SetPixelPosition(Vector3 pos)
{
        m_pixel_pos = pos;
}
 
void ArMarker::SetOrientation(BtQuaternion orientation)
{
        m_orientation = orientation;
}
 
Vector3 ArMarker::GetPosition()
{
  boost::mutex::scoped_lock l(markerMutex);
        return m_pos;
}
 
Vector3 ArMarker::GetPixelPosition()
{
  boost::mutex::scoped_lock l(markerMutex);
  return m_pixel_pos;
}
 
BtQuaternion ArMarker::GetOrientation()
{
  boost::mutex::scoped_lock l(markerMutex);
        return m_orientation;
}
 
unsigned int ArMarker::GetID()
{
        return id;
}
 
ArMarker::MarkerType ArMarker::GetType()
{
  return m_type;
}
 
void ArMarker::SetVisible(bool visible)
{
  //boost::mutex::scoped_lock l(markerMutex);
        m_visible = visible;
}
 
bool ArMarker::IsVisible()
{
  boost::mutex::scoped_lock l(markerMutex);
  boost::shared_lock< boost::shared_mutex > lockList(ArManager::GetInstance()->listMutex);
        return m_visible;
}
 
void ArMarker::SetSize(float size)
{
  boost::mutex::scoped_lock l(markerMutex);
  m_size = size;
}
 
float ArMarker::GetSize()
{
  boost::mutex::scoped_lock l(markerMutex);
  return m_size;
}
 
void ArMarker::Draw(cv::Mat image)
{
  boost::mutex::scoped_lock l(markerMutex);
  if ((*this).size()!=4)
    return;
  
  cv::Scalar color(255, 0, 0);
  int lineWidth = 1;
 
  cv::line(image, (*this)[0], (*this)[1], color, lineWidth, cv::LINE_AA);
  cv::line(image, (*this)[1], (*this)[2], color, lineWidth, cv::LINE_AA);
  cv::line(image, (*this)[2], (*this)[3], color, lineWidth, cv::LINE_AA);
  cv::line(image, (*this)[3], (*this)[0], color, lineWidth, cv::LINE_AA);
  cv::rectangle(image, (*this)[0]-cv::Point2f(2,2), (*this)[0]+cv::Point2f(2,2), cv::Scalar(0,0,255), lineWidth, cv::LINE_AA);
  cv::rectangle(image, (*this)[1]-cv::Point2f(2,2), (*this)[1]+cv::Point2f(2,2), cv::Scalar(0,255,0), lineWidth, cv::LINE_AA);
  cv::rectangle(image, (*this)[2]-cv::Point2f(2,2), (*this)[2]+cv::Point2f(2,2), cv::Scalar(255,0,0), lineWidth, cv::LINE_AA);
 
  char cad[100];
  sprintf(cad, "id=%d", id);
 
  //determine the centroid
  cv::Point cent(0,0);
  for (int i = 0; i < 4; i++)
  {
    cent.x += static_cast<int>((*this)[i].x);
    cent.y += static_cast<int>((*this)[i].y);
  }
  cent.x/= 4;
  cent.y/= 4;
 
  cv::putText(image, cad, cent, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255-color[0], 255-color[1], 255-color[2]), 2);
}
 
void ArMarker::SetCoords(std::vector<cv::Point2f> coords)
{
  if (coords.size() < 4)
    return;
 
  for (unsigned int i = 0; i < 4; i++)
    (*this)[i] = coords[i];
}
 
void ArMarker::Update(ArCameraParam& camparam, aruco::Marker& marker, bool reverse)
{
  /* Get Matrix*/
  double modelview_matrix[16];
  
  try
  {
    //recalculate Rvec and Tvec with marker size
    marker.calculateExtrinsics(m_size, camparam.camParam);
    marker.glGetModelViewMatrix(modelview_matrix);
  }
  catch(std::exception&)
  {
    return;
  }
 
  if(marker.size() == 4)
    SetCoords(marker);
 
  //determine the centroid
  Vector3 pixelPosition(0.0,0.0,0.0);
  for (int j=0;j<4;j++)
  {
    pixelPosition.x += marker[j].x;
    pixelPosition.y += marker[j].y;
  }
 
  pixelPosition.z = sqrt(pow((marker[1].x - marker[0].x),2) + pow((marker[1].y - marker[0].y),2));
  pixelPosition.z += sqrt(pow((marker[2].x - marker[1].x),2) + pow((marker[2].y - marker[1].y),2));
  pixelPosition.z += sqrt(pow((marker[3].x - marker[2].x),2) + pow((marker[3].y - marker[2].y),2));
  pixelPosition.z += sqrt(pow((marker[0].x - marker[3].x),2) + pow((marker[0].y - marker[3].y),2));
 
  pixelPosition.x/=4.0f;
  pixelPosition.y/=4.0f;
  pixelPosition.z/=4.0f;
 
  //cv::circle(image, Point(pixelPosition.x, pixelPosition.y),pixelPosition.z/2,cv::Scalar(255,0,255));
 
  SetPixelPosition(pixelPosition);
  SetVisible(true);
  Vector3 offset = camparam.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));// *BtQuaternion(sqrt(0.5f), 0.0f, sqrt(0.5f), 0.0f));
}
 
/*
void ArMarker::Update(cv::Mat frame, cv::Mat color, aruco::CameraParameters& camparam, bool reverse)
{
  if(!m_bUpdating)
  {
    m_lastCamParam = camparam;
    frame.copyTo(m_lastFrame);
    color.copyTo(m_lastColor);
    m_lastReverse = reverse;
    m_needUpdate = false;
 
    // Run thread
    try
    {
      m_thread_lock.lock();
      SService* service = SService::GetInstance();
      service->addWork(boost::bind(&ArMarker::DetectFeaturedThread, this));
    }
    catch(std::exception&)
    {
      m_thread_lock.unlock();
    }
  }
}
 
void ArMarker::DetectFeaturedThread()
{
  boost::mutex::scoped_lock l(killMutex);
  if (m_fmarker)
  {
    m_bUpdating = true;
    //bool found = (m_visible && m_fmarker->detectMotionFlow(m_lastFrame)) ? true : m_fmarker->detectFeatured(m_lastFrame);
    bool found = m_fmarker->detectFeatured(m_lastFrame, m_lastColor, m_lastCamParam.CamSize);
    if (found)
    {
      m_bUpdating = false;
 
      //Get Matrix
      double modelview_matrix[16];
 
      try
      {
        //recalculate Rvec and Tvec with marker size
        m_fmarker->calculateFeaturedExtrinsics(m_size, m_lastCamParam);
        m_fmarker->glGetModelViewMatrix(modelview_matrix);
      }
      catch (std::exception&)
      {
        m_thread_lock.unlock();
        return;
      }
 
      std::vector<cv::Point2f> coords;
      if (m_fmarker->size() == 4)
      {
        for (int k = 0; k < 4; ++k)
          coords.push_back(m_fmarker->at(k));
        SetCoords(coords);
      }
 
      //determine the centroid
      Vector3 pixelPosition(0.0,0.0,0.0);
      for (int j=0;j<4;j++)
      {
        pixelPosition.x += m_fmarker->at(j).x;
        pixelPosition.y += m_fmarker->at(j).y; 
      }
 
      pixelPosition.z = sqrt(pow((m_fmarker->at(1).x - m_fmarker->at(0).x),2) + pow((m_fmarker->at(1).y - m_fmarker->at(0).y),2));
      pixelPosition.z += sqrt(pow((m_fmarker->at(2).x - m_fmarker->at(1).x),2) + pow((m_fmarker->at(2).y - m_fmarker->at(1).y),2));
      pixelPosition.z += sqrt(pow((m_fmarker->at(3).x - m_fmarker->at(2).x),2) + pow((m_fmarker->at(3).y - m_fmarker->at(2).y),2));
      pixelPosition.z += sqrt(pow((m_fmarker->at(0).x - m_fmarker->at(3).x),2) + pow((m_fmarker->at(0).y - m_fmarker->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>(m_lastReverse ? -modelview_matrix[12] : modelview_matrix[12]), static_cast<float>(modelview_matrix[13]), static_cast<float>(modelview_matrix[14])));
      SetOrientation(BtQuaternion::FromRotationMatrix(modelview_matrix, m_lastReverse));
      SetVisible(true);
    }
    else
    {
      SetVisible(false);
    }
  }
  m_needUpdate = true;
  m_bUpdating = false;
  m_thread_lock.unlock();
}
*/
 
void ArMarker::rotateXAxis(cv::Mat& rotation)
{
  cv::Mat R(3, 3, CV_32F);
  cv::Rodrigues(rotation, R);
  // create a rotation matrix for x axis
  cv::Mat RX = cv::Mat::eye(3, 3, CV_32F);
  const float angleRad = 3.14159265359f / 2.f;
  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;
  // finally, the the rodrigues back
  cv::Rodrigues(R, rotation);
}
 
std::vector<cv::Point2f> ArMarker::GetCorners()
{
  return *this;
}