vpTemplateTrackerZone.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 <visp3/core/vpConfig.h>

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

#include <visp3/tt/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);
          }
        }
      }

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

    vpTime::wait(20);
    vpDisplay::flush(I);
  } while (!end);

  initFromPoints(I, vip, delaunay);
}

void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
                                           bool delaunay)
{
  if (delaunay) {
    if (vip.size() == 3) {
      initFromPoints(I, vip, false);
    } else if (vip.size() == 4) {
      std::vector<vpImagePoint> vip_delaunay;
      vip_delaunay.push_back(vip[0]);
      vip_delaunay.push_back(vip[1]);
      vip_delaunay.push_back(vip[2]);
      vip_delaunay.push_back(vip[2]);
      vip_delaunay.push_back(vip[3]);
      vip_delaunay.push_back(vip[0]);
      initFromPoints(I, vip_delaunay, false);
    } else {
#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(0, 0, I.getWidth(), I.getHeight());

      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, 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, 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;
}

double vpTemplateTrackerZone::getArea() const
{
  double area = 0;
  vpTemplateTrackerTriangle triangle;
  for (unsigned int i = 0; i < getNbTriangle(); i++) {
    getTriangle(i, triangle);
    area += triangle.getArea();
  }
  return area;
}