vpDot.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:
 * Track a white dot.
 *
 * Authors:
 * Eric Marchand
 * Fabien Spindler
 * Aurelien Yol
 *
 *****************************************************************************/

/*
  \file vpDot.cpp
  \brief Track a white dot
*/

#include <visp3/blob/vpDot.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpTrackingException.h>

#include <vector>

/*
  \class vpDot
  \brief Track a white dot
*/

/* spiral size for the dot search */
const unsigned int vpDot::SPIRAL_SEARCH_SIZE = 350;

void vpDot::init()
{
  cog.set_u(0);
  cog.set_v(0);

  compute_moment = false;
  graphics = false;
  thickness = 1;
  maxDotSizePercentage = 0.25; // 25 % of the image size

  mean_gray_level = 0;
  gray_level_min = 128;
  gray_level_max = 255;
  grayLevelPrecision = 0.85;
  gamma = 1.5;

  m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu02 = mu20 = 0;

  connexityType = CONNEXITY_4;

  u_min = u_max = v_min = v_max = 0;

  gray_level_out = 0;
  nbMaxPoint = 0;
}

vpDot::vpDot()
  : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(),
    ip_edges_list(), connexityType(CONNEXITY_4), cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false),
    thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0), gray_level_min(128),
    gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0)
{
}

vpDot::vpDot(const vpImagePoint &ip)
  : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(),
    ip_edges_list(), connexityType(CONNEXITY_4), cog(ip), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false),
    thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0), gray_level_min(128),
    gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0)
{
}

vpDot::vpDot(const vpDot &d)
  : vpTracker(d), m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.),
    ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4), cog(), u_min(0), u_max(0), v_min(0), v_max(0),
    graphics(false), thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0),
    gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0)
{
  *this = d;
}

vpDot::~vpDot() { ip_connexities_list.clear(); }

vpDot &vpDot::operator=(const vpDot &d)
{
  ip_edges_list = d.ip_edges_list;
  ip_connexities_list = d.ip_connexities_list;
  connexityType = d.connexityType;
  cog = d.getCog();

  u_min = d.u_min;
  v_min = d.v_min;
  u_max = d.u_max;
  v_max = d.v_max;

  graphics = d.graphics;
  thickness = d.thickness;
  maxDotSizePercentage = d.maxDotSizePercentage;
  gray_level_out = d.gray_level_out;
  mean_gray_level = d.mean_gray_level;
  gray_level_min = d.gray_level_min;
  gray_level_max = d.gray_level_max;
  grayLevelPrecision = d.grayLevelPrecision;
  gamma = d.gamma;
  compute_moment = d.compute_moment;
  nbMaxPoint = d.nbMaxPoint;

  m00 = d.m00;
  m01 = d.m01;
  m10 = d.m10;
  m11 = d.m11;
  m02 = d.m02;
  m20 = d.m20;

  mu11 = d.mu11;
  mu20 = d.mu20;
  mu02 = d.mu02;

  return *this;
}

bool vpDot::operator!=(const vpDot &d) const { return (cog != d.getCog()); }

bool vpDot::operator==(const vpDot &d) const { return (cog == d.getCog()); }

void vpDot::setGrayLevelOut()
{
  if (gray_level_min == 0) {
    if (gray_level_max == 255) {
      // gray_level_min = 0 and gray_level_max = 255: this should not occur
      // vpERROR_TRACE("Unable to choose a good \"out\" level") ;
      throw(vpTrackingException(vpTrackingException::initializationError, "Unable to choose a good \"out\" level"));
    }
    gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);
  }
}

bool vpDot::connexe(const vpImage<unsigned char> &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog,
                    double &v_cog, double &n)
{
  std::vector<bool> checkTab(I.getWidth() * I.getHeight(), false);
  return connexe(I, u, v, mean_value, u_cog, v_cog, n, checkTab);
}
bool vpDot::connexe(const vpImage<unsigned char> &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog,
                    double &v_cog, double &n, std::vector<bool> &checkTab)
{

  unsigned int width = I.getWidth();
  unsigned int height = I.getHeight();

  // Test if we are in the image
  if ((u >= width) || (v >= height)) {
    // std::cout << "out of bound" << std::endl;
    return false;
  }

  if (checkTab[u + v * I.getWidth()])
    return true;

  vpImagePoint ip;
  ip.set_u(u);
  ip.set_v(v);

  if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max) {
    checkTab[v * I.getWidth() + u] = true;

    ip_connexities_list.push_back(ip);

    u_cog += u;
    v_cog += v;
    n += 1;

    if (n > nbMaxPoint) {
      //      vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
      //            "This threshold can be modified using the
      // setMaxDotSize() "          "method.",          n, n /
      //(I.getWidth() * I.getHeight()),             nbMaxPoint,
      // maxDotSizePercentage) ;

      throw(vpTrackingException(vpTrackingException::featureLostError,
                                "Too many point %lf (%lf%% of image size). "
                                "This threshold can be modified using the setMaxDotSize() "
                                "method.",
                                n, n / (I.getWidth() * I.getHeight()), nbMaxPoint, maxDotSizePercentage));
    }

    // Bounding box update
    if (u < this->u_min)
      this->u_min = u;
    if (u > this->u_max)
      this->u_max = u;
    if (v < this->v_min)
      this->v_min = v;
    if (v > this->v_max)
      this->v_max = v;

    // Mean value of the dot intensities
    mean_value = (mean_value * (n - 1) + I[v][u]) / n;
    if (compute_moment == true) {
      m00++;
      m10 += u;
      m01 += v;
      m11 += (u * v);
      m20 += u * u;
      m02 += v * v;
    }
  } else {
    // std::cout << "not in" << std::endl;
    return false;
  }

  bool edge = false;

  // if((int)u-1 >= 0)
  if (u >= 1)
    if (!checkTab[u - 1 + v * I.getWidth()])
      if (!connexe(I, u - 1, v, mean_value, u_cog, v_cog, n, checkTab))
        edge = true;

  if (u + 1 < I.getWidth())
    if (!checkTab[u + 1 + v * I.getWidth()])
      if (!connexe(I, u + 1, v, mean_value, u_cog, v_cog, n, checkTab))
        edge = true;

  if (v >= 1)
    if (!checkTab[u + (v - 1) * I.getWidth()])
      if (!connexe(I, u, v - 1, mean_value, u_cog, v_cog, n, checkTab))
        edge = true;

  if (v + 1 < I.getHeight())
    if (!checkTab[u + (v + 1) * I.getWidth()])
      if (!connexe(I, u, v + 1, mean_value, u_cog, v_cog, n, checkTab))
        edge = true;

  if (connexityType == CONNEXITY_8) {
    if (v >= 1 && u >= 1)
      if (!checkTab[u - 1 + (v - 1) * I.getWidth()])
        if (!connexe(I, u - 1, v - 1, mean_value, u_cog, v_cog, n, checkTab))
          edge = true;

    if (v >= 1 && u + 1 < I.getWidth())
      if (!checkTab[u + 1 + (v - 1) * I.getWidth()])
        if (!connexe(I, u + 1, v - 1, mean_value, u_cog, v_cog, n, checkTab))
          edge = true;

    if (v + 1 < I.getHeight() && u >= 1)
      if (!checkTab[u - 1 + (v + 1) * I.getWidth()])
        if (!connexe(I, u - 1, v + 1, mean_value, u_cog, v_cog, n, checkTab))
          edge = true;

    if (v + 1 < I.getHeight() && u + 1 < I.getWidth())
      if (!checkTab[u + 1 + (v + 1) * I.getWidth()])
        if (!connexe(I, u + 1, v + 1, mean_value, u_cog, v_cog, n, checkTab))
          edge = true;
  }

  if (edge) {
    ip_edges_list.push_back(ip);
    if (graphics == true) {
      vpImagePoint ip_(ip);
      for (unsigned int t = 0; t < thickness; t++) {
        ip_.set_u(ip.get_u() + t);
        vpDisplay::displayPoint(I, ip_, vpColor::red);
      }
      // vpDisplay::flush(I);
    }
  }

  return true;
}

void vpDot::COG(const vpImage<unsigned char> &I, double &u, double &v)
{
  // Set the maximal number of points considering the maximal dot size
  // image percentage
  nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;

  // segmentation de l'image apres seuillage
  // (etiquetage des composante connexe)
  if (compute_moment)
    m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;

  double u_cog = 0;
  double v_cog = 0;
  double npoint = 0;
  this->mean_gray_level = 0;

  ip_connexities_list.clear();
  ip_edges_list.clear();

  // Initialise the boundig box
  this->u_min = I.getWidth();
  this->u_max = 0;
  this->v_min = I.getHeight();
  this->v_max = 0;

#if 0
  // Original version
  if (  connexe(I, (unsigned int)u, (unsigned int)v,
        gray_level_min, gray_level_max,
        mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
  {
    bool sol = false ;
    unsigned int pas  ;
    for (pas = 2 ; pas <= 25 ; pas ++ )if (sol==false)
    {
      for (int k=-1 ; k <=1 ; k++) if (sol==false)
    for (int l=-1 ; l <=1 ; l++) if (sol==false)
    {
      u_cog = 0 ;
      v_cog = 0 ;
    ip_connexities_list.clear() ;
     
      this->mean_gray_level = 0 ;
      if (connexe(I, (unsigned int)(u+k*pas),(unsigned int)(v+l*pas),
              gray_level_min, gray_level_max,
              mean_gray_level, u_cog, v_cog, npoint) != vpDot::out)
      {
        sol = true ; u += k*pas ; v += l*pas ;
      }
    }
    }
    if (sol == false)
    {
      //vpERROR_TRACE("Dot has been lost") ;
      throw(vpTrackingException(vpTrackingException::featureLostError,
                "Dot has been lost")) ;
    }
  }
#else
  // If the dot is not found, search around using a spiral
  if (!connexe(I, (unsigned int)u, (unsigned int)v, mean_gray_level, u_cog, v_cog, npoint)) {
    bool sol = false;

    unsigned int right = 1;
    unsigned int botom = 1;
    unsigned int left = 2;
    unsigned int up = 2;
    double u_ = u, v_ = v;
    unsigned int k;

    // Spiral search from the center to find the nearest dot
    while ((right < SPIRAL_SEARCH_SIZE) && (sol == false)) {
      for (k = 1; k <= right; k++)
        if (sol == false) {
          u_cog = 0;
          v_cog = 0;
          ip_connexities_list.clear();
          ip_edges_list.clear();

          this->mean_gray_level = 0;
          if (connexe(I, (unsigned int)u_ + k, (unsigned int)(v_), mean_gray_level, u_cog, v_cog, npoint)) {
            sol = true;
            u = u_ + k;
            v = v_;
          }
        }
      u_ += k;
      right += 2;

      for (k = 1; k <= botom; k++)
        if (sol == false) {
          u_cog = 0;
          v_cog = 0;
          ip_connexities_list.clear();
          ip_edges_list.clear();

          this->mean_gray_level = 0;

          if (connexe(I, (unsigned int)(u_), (unsigned int)(v_ + k), mean_gray_level, u_cog, v_cog, npoint)) {
            sol = true;
            u = u_;
            v = v_ + k;
          }
        }
      v_ += k;
      botom += 2;

      for (k = 1; k <= left; k++)
        if (sol == false) {
          u_cog = 0;
          v_cog = 0;
          ip_connexities_list.clear();
          ip_edges_list.clear();

          this->mean_gray_level = 0;

          if (connexe(I, (unsigned int)(u_ - k), (unsigned int)(v_), mean_gray_level, u_cog, v_cog, npoint)) {
            sol = true;
            u = u_ - k;
            v = v_;
          }
        }
      u_ -= k;
      left += 2;

      for (k = 1; k <= up; k++)
        if (sol == false) {
          u_cog = 0;
          v_cog = 0;
          ip_connexities_list.clear();
          ip_edges_list.clear();

          this->mean_gray_level = 0;

          if (connexe(I, (unsigned int)(u_), (unsigned int)(v_ - k), mean_gray_level, u_cog, v_cog, npoint)) {
            sol = true;
            u = u_;
            v = v_ - k;
          }
        }
      v_ -= k;
      up += 2;
    }

    if (sol == false) {
      // vpERROR_TRACE("Dot has been lost") ;
      throw(vpTrackingException(vpTrackingException::featureLostError, "Dot has been lost"));
    }
  }

#endif
  /*
    vpImagePoint ip;
    unsigned int i, j;
    std::list<vpImagePoint>::iterator it;
    for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it
    ++) { ip = *it; i = (unsigned int) ip.get_i(); j = (unsigned int)
    ip.get_j(); I[i][j] = 255 ;
    }*/

  u_cog = u_cog / npoint;
  v_cog = v_cog / npoint;

  u = u_cog;
  v = v_cog;

  // Initialize the threshold for the next call to track()
  double Ip = pow((double)this->mean_gray_level / 255, 1 / gamma);

  if (Ip - (1 - grayLevelPrecision) < 0) {
    gray_level_min = 0;
  } else {
    gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
    if (gray_level_min > 255)
      gray_level_min = 255;
  }
  gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
  if (gray_level_max > 255)
    gray_level_max = 255;

  // vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
  // vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;

  if (npoint < 5) {
    // vpERROR_TRACE("Dot to small") ;
    throw(vpTrackingException(vpTrackingException::featureLostError, "Dot to small"));
  }

  if (npoint > nbMaxPoint) {
    //    vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf
    //    (%lf%%). This threshold can be modified using the setMaxDotSize()
    //    method.",
    //        npoint, npoint / (I.getWidth() * I.getHeight()),
    //        nbMaxPoint, maxDotSizePercentage) ;

    throw(vpTrackingException(vpTrackingException::featureLostError,
                              "Too many point %lf (%lf%%). Max allowed is "
                              "%lf (%lf%%). This threshold can be modified "
                              "using the setMaxDotSize() method.",
                              npoint, npoint / (I.getWidth() * I.getHeight()), nbMaxPoint, maxDotSizePercentage));
  }
}

void vpDot::setMaxDotSize(double percentage)
{
  if (percentage <= 0.0 || percentage > 1.0) {
    // print a warning. We keep the default percentage
    vpTRACE("Max dot size percentage is requested to be set to %lf.",
            "Value should be in ]0:1]. Value will be set to %lf.", percentage, maxDotSizePercentage);
  } else {
    maxDotSizePercentage = percentage;
  }
}

void vpDot::initTracking(const vpImage<unsigned char> &I)
{
  while (vpDisplay::getClick(I, cog) != true)
    ;

  unsigned int i = (unsigned int)cog.get_i();
  unsigned int j = (unsigned int)cog.get_j();

  double Ip = pow((double)I[i][j] / 255, 1 / gamma);

  if (Ip - (1 - grayLevelPrecision) < 0) {
    gray_level_min = 0;
  } else {
    gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
    if (gray_level_min > 255)
      gray_level_min = 255;
  }
  gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
  if (gray_level_max > 255)
    gray_level_max = 255;

  try {
    track(I);
  } catch (const vpException &e) {
    throw(e);
  }
}

void vpDot::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip)
{

  cog = ip;

  unsigned int i = (unsigned int)cog.get_i();
  unsigned int j = (unsigned int)cog.get_j();

  double Ip = pow((double)I[i][j] / 255, 1 / gamma);

  if (Ip - (1 - grayLevelPrecision) < 0) {
    gray_level_min = 0;
  } else {
    gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
    if (gray_level_min > 255)
      gray_level_min = 255;
  }
  gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
  if (gray_level_max > 255)
    gray_level_max = 255;
  try {
    track(I);
  } catch (const vpException &e) {
    throw(e);
  }
}

void vpDot::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int level_min,
                         unsigned int level_max)
{

  cog = ip;

  this->gray_level_min = level_min;
  this->gray_level_max = level_max;

  try {
    track(I);
  } catch (const vpException &e) {
    throw(e);
  }
}

void vpDot::track(const vpImage<unsigned char> &I)
{
  try {
    setGrayLevelOut();
    double u = this->cog.get_u();
    double v = this->cog.get_v();

    COG(I, u, v);

    this->cog.set_u(u);
    this->cog.set_v(v);

    if (compute_moment == true) {
      mu11 = m11 - u * m01;
      mu02 = m02 - v * m01;
      mu20 = m20 - u * m10;
    }

    if (graphics) {
      // display a red cross at the center of gravity's location in the image.
      vpDisplay::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness);
    }

  } catch (const vpException &e) {
    throw(e);
  }
}

void vpDot::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
{
  track(I);

  ip = this->cog;
}

void vpDot::display(const vpImage<unsigned char> &I, vpColor color, unsigned int thick) const
{
  vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thick);
  std::list<vpImagePoint>::const_iterator it;

  for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it) {
    vpDisplay::displayPoint(I, *it, color);
  }
}

void vpDot::setGrayLevelPrecision(const double &precision)
{
  double epsilon = 0.05;
  if (grayLevelPrecision < epsilon) {
    this->grayLevelPrecision = epsilon;
  } else if (grayLevelPrecision > 1) {
    this->grayLevelPrecision = 1.0;
  } else {
    this->grayLevelPrecision = precision;
  }
}

void vpDot::display(const vpImage<unsigned char> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
                    vpColor color, unsigned int thickness)
{
  vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness);
  std::list<vpImagePoint>::const_iterator it;

  for (it = edges_list.begin(); it != edges_list.end(); ++it) {
    vpDisplay::displayPoint(I, *it, color);
  }
}

void vpDot::display(const vpImage<vpRGBa> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
                    vpColor color, unsigned int thickness)
{
  vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness);
  std::list<vpImagePoint>::const_iterator it;

  for (it = edges_list.begin(); it != edges_list.end(); ++it) {
    vpDisplay::displayPoint(I, *it, color);
  }
}

VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpDot &d) { return (os << "(" << d.getCog() << ")"); };