vpDot.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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:
 * 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 = 0;
  m11 = 0;
  m02 = 0;
  m20 = 0;
  m10 = 0;
  m01 = 0;
  mu11 = 0;
  mu02 = 0;
  mu20 = 0;
 
  connexityType = CONNEXITY_4;
 
  u_min = 0;
  u_max = 0;
  v_min = 0;
  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)) {
    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) {
      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 {
    return false;
  }
 
  bool edge = false;
 
  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);
      }
      // --comment: use vpDisplay to 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 = 0;
    m11 = 0;
    m02 = 0;
    m20 = 0;
    m10 = 0;
    m01 = 0;
    mu11 = 0;
    mu20 = 0;
    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, static_cast<unsigned int>(u), static_cast<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, static_cast<unsigned int>(u_) + k, static_cast<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, static_cast<unsigned int>(u_), static_cast<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, static_cast<unsigned int>(u_ - k), static_cast<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, static_cast<unsigned int>(u_), static_cast<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 = static_cast<unsigned int>(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
    if (gray_level_min > 255) {
      gray_level_min = 255;
    }
  }
  gray_level_max = static_cast<unsigned int>(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
  if (gray_level_max > 255) {
    gray_level_max = 255;
  }
 
  // --comment: trace the gray level
 
  if (npoint < 5) {
    // --comment: trace the error using vpERROR_TRACE "Dot to small"
    throw(vpTrackingException(vpTrackingException::featureLostError, "Dot to small"));
  }
 
  if (npoint > nbMaxPoint) {
    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 = static_cast<unsigned int>(cog.get_i());
  unsigned int j = static_cast<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 = static_cast<unsigned int>(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
    if (gray_level_min > 255) {
      gray_level_min = 255;
    }
  }
  gray_level_max = static_cast<unsigned int>(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
  if (gray_level_max > 255) {
    gray_level_max = 255;
  }
 
  track(I);
}
 
void vpDot::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip)
{
 
  cog = ip;
 
  unsigned int i = static_cast<unsigned int>(cog.get_i());
  unsigned int j = static_cast<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 = static_cast<unsigned int>(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
    if (gray_level_min > 255) {
      gray_level_min = 255;
    }
  }
  gray_level_max = static_cast<unsigned int>(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
  if (gray_level_max > 255) {
    gray_level_max = 255;
  }
 
  track(I);
}
 
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;
 
  track(I);
}
 
void vpDot::track(const vpImage<unsigned char> &I)
{
  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);
  }
}
 
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;
 
  std::list<vpImagePoint>::const_iterator ip_edges_list_end = ip_edges_list.end();
  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;
 
  std::list<vpImagePoint>::const_iterator edges_list_end = edges_list.end();
  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;
 
  std::list<vpImagePoint>::const_iterator edges_list_end = edges_list.end();
  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() << ")"); };