vpTemplateTrackerZone.cpp

/****************************************************************************
 *
 * $Id: vpTemplateTrackerZone.cpp 4661 2014-02-10 19:34:58Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Template tracker.
 *
 * Authors:
 * Amaury Dame
 * Aurelien Yol
 * Fabien Spindler
 *
 *****************************************************************************/

#include <limits>   // numeric_limits

#include <visp/vpConfig.h>

#if VISP_HAVE_OPENCV_VERSION >= 0x020300
#include <opencv2/imgproc/imgproc.hpp>
#endif

#include <visp/vpTemplateTrackerZone.h>


vpTemplateTrackerZone::vpTemplateTrackerZone()
  : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
{
}

vpTemplateTrackerZone::vpTemplateTrackerZone(const vpTemplateTrackerZone &z)
  : Zone(), min_x(-1), min_y(-1), max_x(-1), max_y(-1)
{
  *this = z;
}

void vpTemplateTrackerZone::clear()
{
  min_x=-1;
  min_y=-1;
  max_x=-1;
  max_y=-1;

  Zone.clear();
}

vpTemplateTrackerZone & vpTemplateTrackerZone::operator=(const vpTemplateTrackerZone &z)
{
  clear();

  this->copy(z);
  return (*this);
}

void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool delaunay)
{
  Zone.clear();

  std::vector<vpImagePoint> vip;

  bool end = false;

  do {
    vpImagePoint p;
    vpMouseButton::vpMouseButtonType button;
    if (vpDisplay::getClick(I, p, button, false) ) {
      vip.push_back(p);

      vpDisplay::displayCross(I, p, 7, vpColor::red);

      if (vip.size() > 1) {
        if (delaunay) {
          // Draw a line between the 2 last points
          vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
        }
        else {
          if(vip.size() % 3 ==2)
            // draw line between point 2-1
            vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
          else if(vip.size() % 3 ==0) {
            // draw line between point 3-2
            vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
            // draw line between point 3-1
            vpDisplay::displayLine(I, p, vip[vip.size()-3], vpColor::blue, 3);
          }

        }
      }
      vpDisplay::flush(I);

      if (button == vpMouseButton::button3)
        end = true;
    }

    vpTime::wait(20);
  } while(!end);

  initFromPoints(I, vip, delaunay);
}

void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &vip, bool delaunay)
{
  if (delaunay) {
#if VISP_HAVE_OPENCV_VERSION >= 0x020300
    // Init Delaunay
    cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
    for(size_t i=0; i< vip.size(); i++) {
      cv::Point2f fp((float)vip[i].get_u(), (float)vip[i].get_v());
      std::cout << "Click point: " << vip[i] << std::endl;
      subdiv.insert(fp);
    }

    // Compute Delaunay triangulation
    std::vector<cv::Vec6f> triangleList;
    subdiv.getTriangleList(triangleList);

    // Keep only the Delaunay points that are inside the area
    vpRect rect(vip);

    std::vector<vpImagePoint> vip_delaunay;

    for( size_t i = 0; i < triangleList.size(); i++ ) {
      cv::Vec6f t = triangleList[i];
      std::vector<vpImagePoint> p(3);

      p[0].set_uv(t[0], t[1]);
      p[1].set_uv(t[2], t[3]);
      p[2].set_uv(t[4], t[5]);
      if (p[0].inRectangle(rect) && p[1].inRectangle(rect) && p[2].inRectangle(rect)) {
        vip_delaunay.push_back(p[0]);
        vip_delaunay.push_back(p[1]);
        vip_delaunay.push_back(p[2]);
      }
    }

    initFromPoints(I, vip_delaunay, false);
#else
    throw vpException(vpException::functionNotImplementedError,"Delaunay triangulation is not available!");
#endif
  }
  else {
    Zone.clear();
    for(unsigned int i=0; i<vip.size(); i+=3) {
      vpTemplateTrackerTriangle  triangle(vip[i], vip[i+1], vip[i+2]);
      add(triangle);

      vpDisplay::displayLine(I, vip[i],   vip[i+1], vpColor::green, 1);
      vpDisplay::displayLine(I, vip[i+1], vip[i+2], vpColor::green, 1);
      vpDisplay::displayLine(I, vip[i+2], vip[i],   vpColor::green,1);
      vpDisplay::flush(I) ;

      // Update the bounding box
      if((triangle.getMinx()<min_x)||(min_x==-1))
        min_x=(int)triangle.getMinx();
      if((triangle.getMaxx()>max_x)||(max_x==-1))
        max_x=(int)triangle.getMaxx();
      if((triangle.getMiny()<min_y)||(min_y==-1))
        min_y=(int)triangle.getMiny();
      if((triangle.getMaxy()>max_y)||(max_y==-1))
        max_y=(int)triangle.getMaxy();
    }
  }
}

void vpTemplateTrackerZone::add(const vpTemplateTrackerTriangle &t)
{
  Zone.push_back(t);

  // Update the bounding box
  if((t.getMinx()<min_x)||(min_x==-1))
    min_x=(int)t.getMinx();
  if((t.getMaxx()>max_x)||(max_x==-1))
    max_x=(int)t.getMaxx();
  if((t.getMiny()<min_y)||(min_y==-1))
    min_y=(int)t.getMiny();
  if((t.getMaxy()>max_y)||(max_y==-1))
    max_y=(int)t.getMaxy();
}

bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
{
  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
  {
    if(Iterateurvecteur->inTriangle(i,j))
      return true;
  }
  return false;
}

bool vpTemplateTrackerZone::inZone(const double &i,const double &j) const
{
  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
  {
    if(Iterateurvecteur->inTriangle(i,j))
      return true;
  }
  return false;
}

bool vpTemplateTrackerZone::inZone(const int &i,const int &j, unsigned int &id_triangle) const
{
  unsigned int id=0;
  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
  {
    if(Iterateurvecteur->inTriangle(i,j))
    {
      id_triangle=id;
      return true;
    }
    id++;
  }
  return false;
}

bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int &id_triangle) const
{
  unsigned int id=0;
  std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
  for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();Iterateurvecteur++)
  {
    if(Iterateurvecteur->inTriangle(i,j))
    {
      id_triangle=id;
      return true;
    }
    id++;
  }
  return false;
}

void vpTemplateTrackerZone::getTriangle(unsigned int i,vpTemplateTrackerTriangle &T) const
{
  if (i > getNbTriangle()-1)
    throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));

  T = Zone[i];
}
vpTemplateTrackerTriangle vpTemplateTrackerZone::getTriangle(unsigned int i) const
{
  if (i > getNbTriangle()-1)
    throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));

  return Zone[i];
}
vpImagePoint vpTemplateTrackerZone::getCenter() const
{
  double xc=0;
  double yc=0;
  int cpt=0;
  for(int i=min_y;i<max_y;i++)
    for(int j=min_x;j<max_x;j++)
      if(inZone(i,j))
      {
        xc+=j;
        yc+=i;
        cpt ++;
      }
  if(! cpt) {
    throw(vpException(vpException::divideByZeroError,
              "Cannot compute the zone center: size = 0")) ;
  }
  xc=xc/cpt;
  yc=yc/cpt;
  vpImagePoint ip;
  ip.set_uv(xc, yc);
  return ip;
}

int vpTemplateTrackerZone::getMaxx() const
{
  return max_x;
}
int vpTemplateTrackerZone::getMaxy() const
{
  return max_y;
}
int vpTemplateTrackerZone::getMinx() const
{
  return min_x;
}
int vpTemplateTrackerZone::getMiny() const
{
  return min_y;
}

vpRect vpTemplateTrackerZone::getBoundingBox() const
{
  vpRect bbox;
  bbox.setTopLeft(vpImagePoint(min_y, min_x));
  bbox.setBottomRight(vpImagePoint(max_y, max_x));
  return bbox;
}

void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
{
  std::vector<vpImagePoint> ip;
  for (unsigned int i=0; i < Zone.size(); i++) {
    vpTemplateTrackerTriangle triangle;
    Zone[i].getCorners(ip);
    vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
    vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
    vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
  }
}

void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
{
  std::vector<vpImagePoint> ip;
  for (unsigned int i=0; i < Zone.size(); i++) {
    vpTemplateTrackerTriangle triangle;
    Zone[i].getCorners(ip);
    vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
    vpDisplay::displayLine(I, ip[1], ip[2], col, thickness);
    vpDisplay::displayLine(I, ip[2], ip[0], col, thickness);
  }
}

vpTemplateTrackerZone::~vpTemplateTrackerZone()
{
  clear();
}

void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char>& I, unsigned int id,unsigned char gray_level)
{
  assert(id < getNbTriangle());
  vpTemplateTrackerTriangle triangle;
  getTriangle(id, triangle);
  for (int i=0 ; i < (int) I.getHeight() ; i++)
  {
    for (int j=0 ; j < (int) I.getWidth() ; j++)
    {
      if(triangle.inTriangle(i,j))
      {
        I[i][j]=gray_level;
      }
    }
  }
}

vpTemplateTrackerZone vpTemplateTrackerZone::getPyramidDown() const
{
  vpTemplateTrackerZone tempZone;
  vpTemplateTrackerTriangle Ttemp;
  vpTemplateTrackerTriangle TtempDown;
  for(unsigned int i=0;i<getNbTriangle();i++)
  {
    getTriangle(i,Ttemp);
    TtempDown=Ttemp.getPyramidDown();
    tempZone.add(TtempDown);
  }
  return tempZone;
}

void vpTemplateTrackerZone::copy(const vpTemplateTrackerZone& z)
{
  vpTemplateTrackerTriangle triangle;
  for(unsigned int i=0;i<z.getNbTriangle();i++)
  {
    z.getTriangle(i, triangle);
    add(triangle);
    // Update the bounding box
    if((triangle.getMinx()<min_x)||(min_x==-1))
      min_x=(int)triangle.getMinx();
    if((triangle.getMaxx()>max_x)||(max_x==-1))
      max_x=(int)triangle.getMaxx();
    if((triangle.getMiny()<min_y)||(min_y==-1))
      min_y=(int)triangle.getMiny();
    if((triangle.getMaxy()>max_y)||(max_y==-1))
      max_y=(int)triangle.getMaxy();
  }
}

vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
{
  int cpt_pt=0;
  double x_center=0,y_center=0;
  for(int j=0;j<borne_x;j++)
    for(int i=0;i<borne_y;i++)
      if(inZone(i,j))
      {
        x_center+=j;
        y_center+=i;
        cpt_pt++;
      }

  if(! cpt_pt) {
    throw(vpException(vpException::divideByZeroError,
          "Cannot compute the zone center: size = 0")) ;
  }

  x_center=x_center/cpt_pt;
  y_center=y_center/cpt_pt;
  vpImagePoint center;
  center.set_uv(x_center, y_center);
  return center;
}