vpDot2.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:
 * Anthony Saunier
 *
*****************************************************************************/
 
#include <visp3/core/vpDisplay.h>
 
// exception handling
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpTrackingException.h>
 
#include <cmath> // std::fabs
#include <iostream>
#include <limits> // numeric_limits
#include <math.h>
#include <visp3/blob/vpDot2.h>
 
/******************************************************************************
 *
 *      CONSTRUCTORS AND DESTRUCTORS
 *
*****************************************************************************/
void vpDot2::init()
{
  cog.set_u(0);
  cog.set_v(0);
 
  width = 0;
  height = 0;
  surface = 0;
  mean_gray_level = 0;
  gray_level_min = 128;
  gray_level_max = 255;
  grayLevelPrecision = 0.80;
  gamma = 1.5;
 
  sizePrecision = 0.65;
  ellipsoidShapePrecision = 0.65;
  maxSizeSearchDistancePrecision = 0.65;
  setEllipsoidBadPointsPercentage();
  m00 = 0.;
  m11 = 0.;
  m02 = 0.;
  m20 = 0.;
  m10 = 0.;
  m01 = 0.;
  mu11 = 0.;
  mu02 = 0.;
  mu20 = 0.;
 
  bbox_u_min = 0;
  bbox_u_max = 0;
  bbox_v_min = 0;
  bbox_v_max = 0;
 
  firstBorder_u = 0;
  firstBorder_v = 0;
 
  compute_moment = false;
  graphics = false;
  thickness = 1;
}
 
vpDot2::vpDot2()
  : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0),
  surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
  sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
  allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false), graphics(false),
  thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v()
{ }
 
vpDot2::vpDot2(const vpImagePoint &ip)
  : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(ip), width(0), height(0),
  surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
  sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
  allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false), graphics(false),
  thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v()
{ }
 
vpDot2::vpDot2(const vpDot2 &twinDot)
  : vpTracker(twinDot), m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(),
  width(0), height(0), surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0),
  grayLevelPrecision(0.8), gamma(1.5), sizePrecision(0.65), ellipsoidShapePrecision(0.65),
  maxSizeSearchDistancePrecision(0.65), allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(),
  compute_moment(false), graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
  firstBorder_u(0), firstBorder_v()
{
  *this = twinDot;
}
 
vpDot2 &vpDot2::operator=(const vpDot2 &twinDot)
{
  cog = twinDot.cog;
 
  width = twinDot.width;
  height = twinDot.height;
  surface = twinDot.surface;
  gray_level_min = twinDot.gray_level_min;
  gray_level_max = twinDot.gray_level_max;
  mean_gray_level = twinDot.mean_gray_level;
  grayLevelPrecision = twinDot.grayLevelPrecision;
  gamma = twinDot.gamma;
 
  sizePrecision = twinDot.sizePrecision;
  ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision;
  maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision;
  allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_;
  area = twinDot.area;
 
  direction_list = twinDot.direction_list;
  ip_edges_list = twinDot.ip_edges_list;
 
  compute_moment = twinDot.compute_moment;
  graphics = twinDot.graphics;
  thickness = twinDot.thickness;
 
  bbox_u_min = twinDot.bbox_u_min;
  bbox_u_max = twinDot.bbox_u_max;
  bbox_v_min = twinDot.bbox_v_min;
  bbox_v_max = twinDot.bbox_v_max;
 
  firstBorder_u = twinDot.firstBorder_u;
  firstBorder_v = twinDot.firstBorder_v;
 
  m00 = twinDot.m00;
  m01 = twinDot.m01;
  m11 = twinDot.m11;
  m10 = twinDot.m10;
  m02 = twinDot.m02;
  m20 = twinDot.m20;
 
  mu11 = twinDot.mu11;
  mu20 = twinDot.mu20;
  mu02 = twinDot.mu02;
 
  return (*this);
}
 
/******************************************************************************
 *
 *      PUBLIC METHODS
 *****************************************************************************/
 
void vpDot2::display(const vpImage<unsigned char> &I, vpColor color, unsigned int t) const
{
  vpDisplay::displayCross(I, cog, (3 * t) + 8, color, t);
  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 vpDot2::initTracking(const vpImage<unsigned char> &I, unsigned int size)
{
  while (vpDisplay::getClick(I, cog) != true) {
    // block empty waiting user interaction
  }
 
  unsigned int i = static_cast<unsigned int>(cog.get_i());
  unsigned int j = static_cast<unsigned int>(cog.get_j());
 
  double Ip = pow(static_cast<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;
  }
 
  setWidth(size);
  setHeight(size);
 
  track(I);
}
 
void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size)
{
  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(static_cast<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;
  }
 
  setWidth(size);
  setHeight(size);
 
  track(I);
}
 
void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int gray_lvl_min,
                          unsigned int gray_lvl_max, unsigned int size)
{
  cog = ip;
 
  this->gray_level_min = gray_lvl_min;
  this->gray_level_max = gray_lvl_max;
 
  setWidth(size);
  setHeight(size);
 
  track(I);
}
 
void vpDot2::track(const vpImage<unsigned char> &I, bool canMakeTheWindowGrow)
{
  m00 = 0;
  m11 = 0;
  m02 = 0;
  m20 = 0;
  m10 = 0;
  m01 = 0;
 
  // First, we will estimate the position of the tracked point
 
  // Set the search area to the entire image
  setArea(I);
 
  // create a copy of the dot to search
  // This copy can be saw as the previous dot used to check if the current one
  // found with computeParameters() is similar to the previous one (see
  // isValid() function). If the found dot is not similar (or valid), we use
  // this copy to set the current found dot to the previous one (see below).
  vpDot2 wantedDot(*this);
 
  bool found = computeParameters(I, cog.get_u(), cog.get_v());
 
  if (found) {
    // test if the found dot is valid (ie similar to the previous one)
    found = isValid(I, wantedDot);
    if (!found) {
      *this = wantedDot;
      // std::cout << "The found dot is not valid" << std::endl;
    }
  }
 
  if (!found) {
    //     vpDEBUG_TRACE(0, "Search the dot in a biggest window around the
    //     last position"); vpDEBUG_TRACE(0, "Bad computed dot: ");
    //     vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
    //     vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
 
    // if estimation was wrong (get an error tracking), look for the dot
    // closest from the estimation,
    // i.e. search for dots in an a region of interest around the this dot and
    // get the first element in the area.
 
    // first get the size of the search window from the dot size
    double searchWindowWidth = 0.0;
    double searchWindowHeight = 0.0;
 
    if ((std::fabs(getWidth()) <= std::numeric_limits<double>::epsilon()) ||
        (std::fabs(getHeight()) <= std::numeric_limits<double>::epsilon())) {
      searchWindowWidth = 80.;
      searchWindowHeight = 80.;
    }
    else if (canMakeTheWindowGrow) {
      searchWindowWidth = getWidth() * 5;
      searchWindowHeight = getHeight() * 5;
    }
    else {
      searchWindowWidth = getWidth();
      searchWindowHeight = getHeight();
    }
 
    std::list<vpDot2> candidates;
    searchDotsInArea(I, static_cast<int>(this->cog.get_u() - (searchWindowWidth / 2.0)),
                     static_cast<int>(this->cog.get_v() - (searchWindowHeight / 2.0)),
                     static_cast<unsigned int>(searchWindowWidth),
                     static_cast<unsigned int>(searchWindowHeight), candidates);
 
    // if the vector is empty, that mean we didn't find any candidate
    // in the area, return an error tracking.
    if (candidates.empty()) {
      // vpERROR_TRACE("No dot was found") ;
      throw(vpTrackingException(vpTrackingException::featureLostError, "No dot was found"));
    }
 
    // otherwise we've got our dot, update this dot's parameters
    vpDot2 movingDot = candidates.front();
 
    setCog(movingDot.getCog());
    setArea(movingDot.getArea());
    setWidth(movingDot.getWidth());
    setHeight(movingDot.getHeight());
 
    // Update the moments
    m00 = movingDot.m00;
    m01 = movingDot.m01;
    m10 = movingDot.m10;
    m11 = movingDot.m11;
    m20 = movingDot.m20;
    m02 = movingDot.m02;
 
    // Update the bounding box
    bbox_u_min = movingDot.bbox_u_min;
    bbox_u_max = movingDot.bbox_u_max;
    bbox_v_min = movingDot.bbox_v_min;
    bbox_v_max = movingDot.bbox_v_max;
  }
 
  // if this dot is partially out of the image, return an error tracking.
  if (!isInImage(I)) {
    // vpERROR_TRACE("The center of gravity of the dot is not in the image") ;
    throw(vpTrackingException(vpTrackingException::featureLostError,
                              "The center of gravity of the dot is not in the image"));
  }
 
  // Get dots center of gravity
  // unsigned int u = (unsigned int) this->cog.get_u();
  // unsigned int v = (unsigned int) this->cog.get_v();
  // Updates the min and max gray levels for the next iteration
  // double Ip = pow((double)I[v][u]/255,1/gamma);
  double Ip = pow(getMeanGrayLevel() / 255, 1 / gamma);
  // printf("current value of gray level center : %i\n", I[v][u]);
 
  // get Mean Gray Level of I
  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;
  }
 
  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 vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip, bool canMakeTheWindowGrow)
{
  track(I, canMakeTheWindowGrow);
 
  ip = this->cog;
}
 
 
double vpDot2::getWidth() const { return width; }
 
double vpDot2::getHeight() const { return height; }
 
double vpDot2::getArea() const { return fabs(surface); }
 
double vpDot2::getGrayLevelPrecision() const { return grayLevelPrecision; }
 
double vpDot2::getSizePrecision() const { return sizePrecision; }
 
double vpDot2::getEllipsoidShapePrecision() const { return ellipsoidShapePrecision; }
 
double vpDot2::getMaxSizeSearchDistPrecision() const { return maxSizeSearchDistancePrecision; }
 
double vpDot2::getDistance(const vpDot2 &distantDot) const
{
  vpImagePoint cogDistantDot = distantDot.getCog();
  double diff_u = this->cog.get_u() - cogDistantDot.get_u();
  double diff_v = this->cog.get_v() - cogDistantDot.get_v();
  return sqrt((diff_u * diff_u) + (diff_v * diff_v));
}
 
 
void vpDot2::setWidth(const double &w) { this->width = w; }
 
void vpDot2::setHeight(const double &h) { this->height = h; }
 
void vpDot2::setArea(const double &a) { this->surface = a; }
 
void vpDot2::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 vpDot2::setSizePrecision(const double &precision)
{
  if (sizePrecision < 0) {
    this->sizePrecision = 0;
  }
  else if (sizePrecision > 1) {
    this->sizePrecision = 1.0;
  }
  else {
    this->sizePrecision = precision;
  }
}
 
void vpDot2::setEllipsoidShapePrecision(const double &precision)
{
 
  if (ellipsoidShapePrecision < 0) {
    this->ellipsoidShapePrecision = 0;
  }
  else if (ellipsoidShapePrecision > 1) {
    this->ellipsoidShapePrecision = 1.0;
  }
  else {
    this->ellipsoidShapePrecision = precision;
  }
}
 
void vpDot2::setMaxSizeSearchDistPrecision(const double &precision)
{
  double epsilon = 0.05;
  if (maxSizeSearchDistancePrecision < epsilon) {
    this->maxSizeSearchDistancePrecision = epsilon;
  }
  else if (maxSizeSearchDistancePrecision > 1) {
    this->maxSizeSearchDistancePrecision = 1.0;
  }
  else {
    this->maxSizeSearchDistancePrecision = precision;
  }
}
 
void vpDot2::setArea(const vpImage<unsigned char> &I) { setArea(I, 0, 0, I.getWidth(), I.getHeight()); }
 
void vpDot2::setArea(const vpImage<unsigned char> &I, int u, int v, unsigned int w, unsigned int h)
{
  unsigned int image_w = I.getWidth();
  unsigned int image_h = I.getHeight();
 
  // Bounds the area to the image
  if (u < 0) {
    u = 0;
  }
  else if (u >= static_cast<int>(image_w)) {
    u = static_cast<int>(image_w) - 1;
  }
  if (v < 0) {
    v = 0;
  }
  else if (v >= static_cast<int>(image_h)) {
    v = static_cast<int>(image_h) - 1;
  }
 
  if ((static_cast<unsigned int>(u) + w) > image_w) {
    w = image_w - static_cast<unsigned int>(u) - 1;
  }
  if ((static_cast<unsigned int>(v) + h) > image_h) {
    h = image_h - static_cast<unsigned int>(v) - 1;
  }
 
  area.setRect(u, v, w, h);
}
 
void vpDot2::setArea(const vpRect &a) { area = a; }
 
 
void vpDot2::searchDotsInArea(const vpImage<unsigned char> &I, std::list<vpDot2> &niceDots)
{
  searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
}
 
void vpDot2::searchDotsInArea(const vpImage<unsigned char> &I, int area_u, int area_v, unsigned int area_w,
                              unsigned int area_h, std::list<vpDot2> &niceDots)
 
{
  // clear the list of nice dots
  niceDots.clear();
 
  // Fit the input area in the image; we keep only the common part between
  // this area and the image.
  setArea(I, area_u, area_v, area_w, area_h);
 
  // compute the size of the search grid
  unsigned int gridWidth;
  unsigned int gridHeight;
  getGridSize(gridWidth, gridHeight);
 
  if (graphics) {
    // Display the area were the dot is search
    vpDisplay::displayRectangle(I, area, vpColor::blue, false, thickness);
  }
 
#ifdef DEBUG
  vpDisplay::displayRectangle(I, area, vpColor::blue);
  vpDisplay::flush(I);
#endif
  // start the search loop; for all points of the search grid,
  // test if the pixel belongs to a valid dot.
  // if it is so eventually add it to the vector of valid dots.
  std::list<vpDot2> badDotsVector;
  std::list<vpDot2>::iterator itnice;
  std::list<vpDot2>::iterator itbad;
 
  vpDot2 *dotToTest = nullptr;
  vpDot2 tmpDot;
 
  unsigned int area_u_min = static_cast<unsigned int>(area.getLeft());
  unsigned int area_u_max = static_cast<unsigned int>(area.getRight());
  unsigned int area_v_min = static_cast<unsigned int>(area.getTop());
  unsigned int area_v_max = static_cast<unsigned int>(area.getBottom());
 
  unsigned int u, v;
  vpImagePoint cogTmpDot;
 
  for (v = area_v_min; v < area_v_max; v = v + gridHeight) {
    for (u = area_u_min; u < area_u_max; u = u + gridWidth) {
      // if the pixel we're in doesn't have the right color (outside the
      // graylevel interval), no need to check further, just get to the
      // next grid intersection.
      if (!hasGoodLevel(I, u, v)) {
        continue;
      }
 
      // Test if an other germ is inside the bounding box of a dot previously
      // detected
      bool good_germ = true;
 
      itnice = niceDots.begin();
      while ((itnice != niceDots.end()) && (good_germ == true)) {
        tmpDot = *itnice;
 
        cogTmpDot = tmpDot.getCog();
        double u0 = cogTmpDot.get_u();
        double v0 = cogTmpDot.get_v();
        double half_w = tmpDot.getWidth() / 2.;
        double half_h = tmpDot.getHeight() / 2.;
 
        if ((u >= (u0 - half_w)) && (u <= (u0 + half_w)) && (v >= (v0 - half_h)) && (v <= (v0 + half_h))) {
          // Germ is in a previously detected dot
          good_germ = false;
        }
        ++itnice;
      }
 
      if (!good_germ) {
        continue;
      }
 
      // Compute the right border position for this possible germ
      unsigned int border_u;
      unsigned int border_v;
      if (findFirstBorder(I, u, v, border_u, border_v) == false) {
        // germ is not good.
        // Jump all the pixels between v,u and v,
        // dotToTest->getFirstBorder_u()
        u = border_u;
        v = border_v;
        continue;
      }
 
      itbad = badDotsVector.begin();
#define vpBAD_DOT_VALUE (*itbad)
      vpImagePoint cogBadDot;
 
      while ((itbad != badDotsVector.end()) && (good_germ == true)) {
        if ((static_cast<double>(u) >= vpBAD_DOT_VALUE.bbox_u_min) && (static_cast<double>(u) <= vpBAD_DOT_VALUE.bbox_u_max) &&
            (static_cast<double>(v) >= vpBAD_DOT_VALUE.bbox_v_min) && (static_cast<double>(v) <= vpBAD_DOT_VALUE.bbox_v_max)) {
          std::list<vpImagePoint>::const_iterator it_edges = ip_edges_list.begin();
          while ((it_edges != ip_edges_list.end()) && (good_germ == true)) {
            // Test if the germ belong to a previously detected dot:
            // - from the germ go right to the border and compare this
            //   position to the list of pixels of previously detected dots
            cogBadDot = *it_edges;
            if ((std::fabs(border_u - cogBadDot.get_u()) <=
                 (vpMath::maximum(std::fabs(static_cast<double>(border_u)), std::fabs(cogBadDot.get_u())) *
                  std::numeric_limits<double>::epsilon())) &&
                (std::fabs(v - cogBadDot.get_v()) <=
                 (vpMath::maximum(std::fabs(static_cast<double>(v)), std::fabs(cogBadDot.get_v())) *
                  std::numeric_limits<double>::epsilon()))) {
              good_germ = false;
            }
            ++it_edges;
          }
        }
        ++itbad;
      }
#undef vpBAD_DOT_VALUE
 
      if (!good_germ) {
        // Jump all the pixels between v,u and v,
        // dotToTest->getFirstBorder_u()
        u = border_u;
        v = border_v;
        continue;
      }
 
      vpTRACE(4, "Try germ (%d, %d)", u, v);
 
      vpImagePoint germ;
      germ.set_u(u);
      germ.set_v(v);
 
      // otherwise estimate the width, height and surface of the dot we
      // created, and test it.
      if (dotToTest != nullptr) {
        delete dotToTest;
      }
      dotToTest = getInstance();
      dotToTest->setCog(germ);
      dotToTest->setGrayLevelMin(getGrayLevelMin());
      dotToTest->setGrayLevelMax(getGrayLevelMax());
      dotToTest->setGrayLevelPrecision(getGrayLevelPrecision());
      dotToTest->setSizePrecision(getSizePrecision());
      dotToTest->setGraphics(graphics);
      dotToTest->setGraphicsThickness(thickness);
      dotToTest->setComputeMoments(true);
      dotToTest->setArea(area);
      dotToTest->setEllipsoidShapePrecision(ellipsoidShapePrecision);
      dotToTest->setEllipsoidBadPointsPercentage(allowedBadPointsPercentage_);
 
      // first compute the parameters of the dot.
      // if for some reasons this caused an error tracking
      // (dot partially out of the image...), check the next intersection
      if (dotToTest->computeParameters(I) == false) {
        // Jump all the pixels between v,u and v,
        // dotToTest->getFirstBorder_u()
        u = border_u;
        v = border_v;
        continue;
      }
      // if the dot to test is valid,
      if (dotToTest->isValid(I, *this)) {
        vpImagePoint cogDotToTest = dotToTest->getCog();
        // Compute the distance to the center. The center used here is not the
        // area center available by area.getCenter(area_center_u,
        // area_center_v) but the center of the input area which may be
        // partially outside the image.
 
        double area_center_u = (area_u + (area_w / 2.0)) - 0.5;
        double area_center_v = (area_v + (area_h / 2.0)) - 0.5;
 
        double thisDiff_u = cogDotToTest.get_u() - area_center_u;
        double thisDiff_v = cogDotToTest.get_v() - area_center_v;
        double thisDist = sqrt((thisDiff_u * thisDiff_u) + (thisDiff_v * thisDiff_v));
 
        bool stopLoop = false;
        itnice = niceDots.begin();
 
        while ((itnice != niceDots.end()) && (stopLoop == false)) {
          tmpDot = *itnice;
 
          // --comment: epsilon equals 0.001 -- detecte +sieurs points
          double epsilon = 3.0;
          // if the center of the dot is the same than the current
          // don't add it, test the next point of the grid
          cogTmpDot = tmpDot.getCog();
 
          if ((fabs(cogTmpDot.get_u() - cogDotToTest.get_u()) < epsilon) &&
              (fabs(cogTmpDot.get_v() - cogDotToTest.get_v()) < epsilon)) {
            stopLoop = true;
            // Jump all the pixels between v,u and v,
            // tmpDot->getFirstBorder_u()
            u = border_u;
            v = border_v;
            continue;
          }
 
          double otherDiff_u = cogTmpDot.get_u() - area_center_u;
          double otherDiff_v = cogTmpDot.get_v() - area_center_v;
          double otherDist = sqrt((otherDiff_u * otherDiff_u) + (otherDiff_v * otherDiff_v));
 
          // if the distance of the curent vector element to the center
          // is greater than the distance of this dot to the center,
          // then add this dot before the current vector element.
          if (otherDist > thisDist) {
            niceDots.insert(itnice, *dotToTest);
            ++itnice;
            stopLoop = true;
            // Jump all the pixels between v,u and v,
            // tmpDot->getFirstBorder_u()
            u = border_u;
            v = border_v;
            continue;
          }
          ++itnice;
        }
        vpTRACE(4, "End while (%d, %d)", u, v);
 
        // if we reached the end of the vector without finding the dot
        // or inserting it, insert it now.
        if ((itnice == niceDots.end()) && (stopLoop == false)) {
          niceDots.push_back(*dotToTest);
        }
      }
      else {
        // Store bad dots
        badDotsVector.push_front(*dotToTest);
      }
    }
  }
  if (dotToTest != nullptr) {
    delete dotToTest;
  }
}
 
bool vpDot2::isValid(const vpImage<unsigned char> &I, const vpDot2 &wantedDot)
{
  double size_precision = wantedDot.getSizePrecision();
  double ellipsoidShape_precision = wantedDot.getEllipsoidShapePrecision();
 
  //
  // First, check the width, height and surface of the dot. Those parameters
  // must be the same.
  //
  // if (   (wantedDot.getWidth()   != 0)
  //  && (wantedDot.getHeight()  != 0)
  //  && (wantedDot.getArea() != 0) )
  if ((std::fabs(wantedDot.getWidth()) > std::numeric_limits<double>::epsilon()) &&
      (std::fabs(wantedDot.getHeight()) > std::numeric_limits<double>::epsilon()) &&
      (std::fabs(wantedDot.getArea()) > std::numeric_limits<double>::epsilon())) {
    if (std::fabs(size_precision) > std::numeric_limits<double>::epsilon()) {
      double epsilon = 0.001;
#ifdef DEBUG
      std::cout << "test size precision......................\n";
      std::cout << "wanted dot: "
        << "w=" << wantedDot.getWidth() << " h=" << wantedDot.getHeight() << " s=" << wantedDot.getArea()
        << " precision=" << size_precision << " epsilon=" << epsilon << std::endl;
      std::cout << "dot found: "
        << "w=" << getWidth() << " h=" << getHeight() << " s=" << getArea() << std::endl;
#endif
 
      if ((((wantedDot.getWidth() * size_precision) - epsilon) < getWidth()) == false) {
        vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)", cog.get_u(), cog.get_v());
#ifdef DEBUG
        printf("Bad width > for dot (%g, %g)\n", cog.get_u(), cog.get_v());
#endif
        return false;
      }
 
      if ((getWidth() < (wantedDot.getWidth() / (size_precision + epsilon))) == false) {
        vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)", cog.get_u(), cog.get_v());
#ifdef DEBUG
        printf("Bad width %g > %g for dot (%g, %g)\n", getWidth(), wantedDot.getWidth() / (size_precision + epsilon),
               cog.get_u(), cog.get_v());
#endif
        return false;
      }
 
      if ((((wantedDot.getHeight() * size_precision) - epsilon) < getHeight()) == false) {
        vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)", cog.get_u(), cog.get_v());
#ifdef DEBUG
        printf("Bad height %g > %g for dot (%g, %g)\n", wantedDot.getHeight() * size_precision - epsilon, getHeight(),
               cog.get_u(), cog.get_v());
#endif
        return false;
      }
 
      if ((getHeight() < (wantedDot.getHeight() / (size_precision + epsilon))) == false) {
        vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)", cog.get_u(), cog.get_v());
#ifdef DEBUG
        printf("Bad height %g > %g for dot (%g, %g)\n", getHeight(), wantedDot.getHeight() / (size_precision + epsilon),
               cog.get_u(), cog.get_v());
#endif
        return false;
      }
 
      if ((((wantedDot.getArea() * (size_precision * size_precision)) - epsilon) < getArea()) == false) {
        vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)", cog.get_u(), cog.get_v());
#ifdef DEBUG
        printf("Bad surface %g > %g for dot (%g, %g)\n",
               wantedDot.getArea() * (size_precision * size_precision) - epsilon, getArea(), cog.get_u(), cog.get_v());
#endif
        return false;
      }
 
      if ((getArea() < (wantedDot.getArea() / ((size_precision * size_precision) + epsilon))) == false) {
        vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)", cog.get_u(), cog.get_v());
#ifdef DEBUG
        printf("Bad surface %g < %g for dot (%g, %g)\n", getArea(),
               wantedDot.getArea() / (size_precision * size_precision + epsilon), cog.get_u(), cog.get_v());
#endif
        return false;
      }
    }
  }
  //
  // Now we can proceed to more advanced (and costy) checks.
  // First check there is a white (>level) elipse within dot
  // Then check the dot is surrounded by a black ellipse.
  //
  int nb_point_to_test = 20; // Nb points to test on inner and outside ellipsoid
  int nb_bad_points = 0;
  int nb_max_bad_points = static_cast<int>(nb_point_to_test * allowedBadPointsPercentage_);
  double step_angle = (2 * M_PI) / nb_point_to_test;
 
  // --comment: if ellipsoidShape_precision diff 0 and compute_moment is true
  if ((std::fabs(ellipsoidShape_precision) > std::numeric_limits<double>::epsilon()) && compute_moment) {
    // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 15
 
    /*
    // -comment: mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00
    // -comment:      = m11 - m10 * m01 / m00
    // -comment: mu20 = m20 - m00 * xg^2 = m20 - m00 * m10/m00 * m10/m00
    // -comment:      = m20 - m10^2 / m00
    // -comment: mu02 = m02 - m01^2 / m00
    // -comment: alpha = 1/2 arctan( 2 * mu11 / (mu20 - mu02) )
    //
    // -comment: a1^2 = 2 / m00 * (mu02 + mu20 + sqrt( (mu20 - mu02)^2 + 4mu11^2) )
    //
    // -comment: a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) )
    */
    // we compute parameters of the estimated ellipse
    double tmp1 = (((m01 * m01) - (m10 * m10)) / m00) + (m20 - m02);
    double tmp2 = m11 - ((m10 * m01) / m00);
    double Sqrt = sqrt((tmp1 * tmp1) + (4 * tmp2 * tmp2));
    double a1 = sqrt((2 / m00) * (((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00)) + Sqrt));
    double a2 = sqrt((2 / m00) * (((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00)) - Sqrt));
    double alpha = 0.5 * atan2(2 * ((m11 * m00) - (m10 * m01)), ((((m20 - m02) * m00) - (m10 * m10)) + (m01 * m01)));
 
    // to be able to track small dots, minorize the ellipsoid radius for the
    // inner test
    a1 -= 1.0;
    a2 -= 1.0;
 
    double innerCoef = ellipsoidShape_precision;
    unsigned int u, v;
    double cog_u = this->cog.get_u();
    double cog_v = this->cog.get_v();
 
    vpImagePoint ip;
    nb_bad_points = 0;
    for (double theta = 0.; theta < (2 * M_PI); theta += step_angle) {
      u = static_cast<unsigned int>(cog_u + (innerCoef * ((a1 * cos(alpha) * cos(theta)) - (a2 * sin(alpha) * sin(theta)))));
      v = static_cast<unsigned int>(cog_v + (innerCoef * ((a1 * sin(alpha) * cos(theta)) + (a2 * cos(alpha) * sin(theta)))));
      if (!this->hasGoodLevel(I, u, v)) {
#ifdef DEBUG
        printf("Inner circle pixel (%u, %u) has bad level for dot (%g, %g): "
               "%d not in [%u, %u]\n",
               u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
#endif
        ++nb_bad_points;
      }
      if (graphics) {
        for (unsigned int t = 0; t < thickness; ++t) {
          ip.set_u(u + t);
          ip.set_v(v);
          vpDisplay::displayPoint(I, ip, vpColor::green);
        }
      }
#ifdef DEBUG
      vpDisplay::displayPoint(I, ip, vpColor::green);
      vpDisplay::flush(I);
#endif
    }
    if (nb_bad_points > nb_max_bad_points) {
#ifdef DEBUG
      printf("Inner ellipse has %d bad points. Max allowed is %d\n", nb_bad_points, nb_max_bad_points);
#endif
      return false;
    }
    // to be able to track small dots, maximize the ellipsoid radius for the
    // inner test
    a1 += 2.0;
    a2 += 2.0;
 
    double outCoef = 2 - ellipsoidShape_precision; // --comment: 1.6
    nb_bad_points = 0;
    for (double theta = 0.; theta < (2 * M_PI); theta += step_angle) {
      u = static_cast<unsigned int>(cog_u + (outCoef * ((a1 * cos(alpha) * cos(theta)) - (a2 * sin(alpha) * sin(theta)))));
      v = static_cast<unsigned int>(cog_v + (outCoef * ((a1 * sin(alpha) * cos(theta)) + (a2 * cos(alpha) * sin(theta)))));
#ifdef DEBUG
      // vpDisplay::displayRectangle(I, area, vpColor::yellow);
      vpDisplay::displayCross(I, (int)v, (int)u, 7, vpColor::purple);
      vpDisplay::flush(I);
#endif
      // If outside the area, continue
      if ((static_cast<double>(u) < area.getLeft()) ||
          (static_cast<double>(u) > area.getRight()) ||
          (static_cast<double>(v) < area.getTop()) ||
          (static_cast<double>(v) > area.getBottom())) {
        // continue
      }
      else {
        if (!this->hasReverseLevel(I, u, v)) {
#ifdef DEBUG
          printf("Outside circle pixel (%u, %u) has bad level for dot (%g, "
                 "%g): %d not in [%u, %u]\n",
                 u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
#endif
          ++nb_bad_points;
        }
        if (graphics) {
          for (unsigned int t = 0; t < thickness; ++t) {
            ip.set_u(u + t);
            ip.set_v(v);
 
            vpDisplay::displayPoint(I, ip, vpColor::green);
          }
        }
      }
    }
  }
  if (nb_bad_points > nb_max_bad_points) {
#ifdef DEBUG
    printf("Outside ellipse has %d bad points. Max allowed is %d\n", nb_bad_points, nb_max_bad_points);
#endif
    return false;
  }
 
  return true;
}
 
bool vpDot2::hasGoodLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const
{
  if (!isInArea(u, v)) {
    return false;
  }
 
  if ((I[v][u] >= gray_level_min) && (I[v][u] <= gray_level_max)) {
    return true;
  }
  else {
    return false;
  }
}
 
bool vpDot2::hasReverseLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const
{
 
  if (!isInArea(u, v)) {
    return false;
  }
 
  if ((I[v][u] < gray_level_min) || (I[v][u] > gray_level_max)) {
    return true;
  }
  else {
    return false;
  }
}
 
vpDot2 *vpDot2::getInstance() { return new vpDot2(); }
 
void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const { freeman_chain = direction_list; }
 
/******************************************************************************
 *
 *      PRIVATE METHODS
 *
 ******************************************************************************/
 
bool vpDot2::computeParameters(const vpImage<unsigned char> &I, const double &v_u, const double &v_v)
{
  direction_list.clear();
  ip_edges_list.clear();
 
  double est_u = v_u; // estimated
  double est_v = v_v;
 
  // if u has default value, set it to the actual center value
  // if( est_u == -1.0 )
  if (std::fabs(est_u + 1.0) <= (vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits<double>::epsilon())) {
    est_u = this->cog.get_u();
  }
 
  // if v has default value, set it to the actual center value
  // if( est_v == -1.0 )
  if (std::fabs(est_v + 1.0) <= (vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits<double>::epsilon())) {
    est_v = this->cog.get_v();
  }
 
  // if the estimated position of the dot is out of the image, not need to
  // continue, return an error tracking
  if (!isInArea(static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v))) {
    vpDEBUG_TRACE(3,
                  "Initial pixel coordinates (%d, %d) for dot tracking are "
                  "not in the area",
                  static_cast<int>(est_u), static_cast<int>(est_v));
    return false;
  }
 
  bbox_u_min = static_cast<int>(I.getWidth());
  bbox_u_max = 0;
  bbox_v_min = static_cast<int>(I.getHeight());
  bbox_v_max = 0;
 
  // if the first point doesn't have the right level then there's no point to
  // continue.
  if (!hasGoodLevel(I, static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v))) {
    vpDEBUG_TRACE(3, "Can't find a dot from pixel (%d, %d) coordinates", static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v));
    return false;
  }
 
  // find the border
 
  if (!findFirstBorder(I, static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v), this->firstBorder_u, this->firstBorder_v)) {
 
    vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates", static_cast<int>(est_u), static_cast<int>(est_v));
    return false;
  }
 
  unsigned int dir = 6;
 
  // Determine the first element of the Freeman chain
  computeFreemanChainElement(I, this->firstBorder_u, this->firstBorder_v, dir);
  unsigned int firstDir = dir;
 
  // if we are now out of the image, return an error tracking
  if (!isInArea(this->firstBorder_u, this->firstBorder_v)) {
    vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area", this->firstBorder_u,
                  this->firstBorder_v);
    return false;
  }
 
  // store the new direction and dot border coordinates.
  direction_list.push_back(dir);
  vpImagePoint ip;
  ip.set_u(this->firstBorder_u);
  ip.set_v(this->firstBorder_v);
 
  ip_edges_list.push_back(ip);
 
  int border_u = static_cast<int>(this->firstBorder_u);
  int border_v = static_cast<int>(this->firstBorder_v);
  int du, dv;
  float dS, dMu, dMv, dMuv, dMu2, dMv2;
  m00 = 0.0;
  m10 = 0.0;
  m01 = 0.0;
  m11 = 0.0;
  m20 = 0.0;
  m02 = 0.0;
  // while we didn't come back to the first point, follow the border
  do {
    // if it was asked, show the border
    if (graphics) {
      for (int t = 0; t < static_cast<int>(thickness); ++t) {
        ip.set_u(border_u + t);
        ip.set_v(border_v);
 
        vpDisplay::displayPoint(I, ip, vpColor::red);
      }
    }
#ifdef DEBUG
    vpDisplay::displayPoint(I, border_v, border_u, vpColor::red);
    vpDisplay::flush(I);
#endif
    // Determine the increments for the parameters
    computeFreemanParameters(border_u, border_v, dir, du, dv,
                             dS,                // surface
                             dMu, dMv,          // first order moments
                             dMuv, dMu2, dMv2); // second order moment
 
    // Update the parameters
    border_u += du; // Next position on the border
    border_v += dv;
    m00 += dS;  // enclosed area
    m10 += dMu; // First order moment along v axis
    m01 += dMv; // First order moment along u axis
    if (compute_moment) {
      m11 += dMuv; // Second order moment
      m20 += dMu2; // Second order moment along v axis
      m02 += dMv2; // Second order moment along u axis
    }
    // if we are now out of the image, return an error tracking
    if (!isInArea(static_cast<unsigned int>(border_u), static_cast<unsigned int>(border_v))) {
 
      vpDEBUG_TRACE(3, "Dot (%d, %d) is not in the area", border_u, border_v);
      // Can Occur on a single pixel dot located on the top border
      return false;
    }
 
    // store the new direction and dot border coordinates.
 
    direction_list.push_back(dir);
 
    ip.set_u(border_u);
    ip.set_v(border_v);
    ip_edges_list.push_back(ip);
 
    // update the extreme point of the dot.
    if (border_v < bbox_v_min) {
      bbox_v_min = border_v;
    }
    if (border_v > bbox_v_max) {
      bbox_v_max = border_v;
    }
    if (border_u < bbox_u_min) {
      bbox_u_min = border_u;
    }
    if (border_u > bbox_u_max) {
      bbox_u_max = border_u;
    }
 
    // move around the tracked entity by following the border.
    if (computeFreemanChainElement(I, static_cast<unsigned int>(border_u), static_cast<unsigned int>(border_v), dir) == false) {
      vpDEBUG_TRACE(3, "Can't compute Freeman chain for dot (%d, %d)", border_u, border_v);
      return false;
    }
 
    //     vpTRACE("border_u: %d border_v: %d dir: %d", border_u, border_v,
    //     dir);
 
  } while (((getFirstBorder_u() != static_cast<unsigned int>(border_u)) || (getFirstBorder_v() != static_cast<unsigned int>(border_v)) ||
            (firstDir != dir)) &&
           isInArea(static_cast<unsigned int>(border_u), static_cast<unsigned int>(border_v)));
 
#ifdef VP_DEBUG
#if VP_DEBUG_MODE == 3
  vpDisplay::flush(I);
#endif
#endif
 
  // if the surface is one or zero , the center of gravity wasn't properly
  // detected. Return an error tracking.
  // if( m00 == 0 || m00 == 1 )
  if ((std::fabs(m00) <= std::numeric_limits<double>::epsilon()) ||
      (std::fabs(m00 - 1.) <= (vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits<double>::epsilon()))) {
    vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected");
    return false;
  }
  else // compute the center
  {
    // this magic formula gives the coordinates of the center of gravity
    double tmpCenter_u = m10 / m00;
    double tmpCenter_v = m01 / m00;
 
    // Updates the second order centered moments
    if (compute_moment) {
      mu11 = m11 - (tmpCenter_u * m01);
      mu02 = m02 - (tmpCenter_v * m01);
      mu20 = m20 - (tmpCenter_u * m10);
    }
 
    cog.set_u(tmpCenter_u);
    cog.set_v(tmpCenter_v);
  }
 
  width = (bbox_u_max - bbox_u_min) + 1;
  height = (bbox_v_max - bbox_v_min) + 1;
  surface = m00;
 
  computeMeanGrayLevel(I);
  return true;
}
 
bool vpDot2::findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
                             unsigned int &border_u, unsigned int &border_v)
{
  // find the border
 
  // NOTE:
  // from here we use int and not double. This is because we don't have
  // rounding problems and it's actually more a trouble than smth else to
  // work with double when navigating around the dot.
  border_u = u;
  border_v = v;
  double epsilon = 0.001;
 
#ifdef DEBUG
  std::cout << "gray level: " << gray_level_min << " " << gray_level_max << std::endl;
#endif
  while (hasGoodLevel(I, border_u + 1, border_v) && (border_u < area.getRight()) /*I.getWidth()*/) {
    // if the width of this dot was initialised and we already crossed the dot
    // on more than the max possible width, no need to continue, return an
    // error tracking
    if ((getWidth() > 0) && ((border_u - u) > ((getWidth() / getMaxSizeSearchDistPrecision()) + epsilon))) {
      vpDEBUG_TRACE(3,
                    "The found dot (%d, %d, %d) has a greater width than the "
                    "required one",
                    u, v, border_u);
      return false;
    }
#ifdef DEBUG
    vpDisplay::displayPoint(I, (int)border_v, (int)border_u + 1, vpColor::green);
    vpDisplay::flush(I);
#endif
 
    ++border_u;
  }
  return true;
}
 
bool vpDot2::computeFreemanChainElement(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
                                        unsigned int &element)
{
 
  if (hasGoodLevel(I, u, v)) {
    unsigned int v_u = u;
    unsigned int v_v = v;
    // get the point on the right of the point passed in
    updateFreemanPosition(v_u, v_v, (element + 2) % 8);
    if (hasGoodLevel(I, v_u, v_v)) {
      element = (element + 2) % 8; // turn right
    }
    else {
      unsigned int v_u1 = u;
      unsigned int v_v1 = v;
      updateFreemanPosition(v_u1, v_v1, (element + 1) % 8);
 
      if (hasGoodLevel(I, v_u1, v_v1)) {
        element = (element + 1) % 8; // turn diag right
      }
      else {
        unsigned int v_u2 = u;
        unsigned int v_v2 = v;
        updateFreemanPosition(v_u2, v_v2, element); // same direction
 
        if (hasGoodLevel(I, v_u2, v_v2)) {
          // element = element;      // keep same dir
        }
        else {
          unsigned int v_u3 = u;
          unsigned int v_v3 = v;
          updateFreemanPosition(v_u3, v_v3, (element + 7) % 8); // diag left
 
          if (hasGoodLevel(I, v_u3, v_v3)) {
            element = (element + 7) % 8; // turn diag left
          }
          else {
            unsigned int v_u4 = u;
            unsigned int v_v4 = v;
            updateFreemanPosition(v_u4, v_v4, (element + 6) % 8); // left
 
            if (hasGoodLevel(I, v_u4, v_v4)) {
              element = (element + 6) % 8; // turn left
            }
            else {
              unsigned int v_u5 = u;
              unsigned int v_v5 = v;
              updateFreemanPosition(v_u5, v_v5, (element + 5) % 8); // left
 
              if (hasGoodLevel(I, v_u5, v_v5)) {
                element = (element + 5) % 8; // turn diag down
              }
              else {
                unsigned int v_u6 = u;
                unsigned int v_v6 = v;
                updateFreemanPosition(v_u6, v_v6, (element + 4) % 8); // left
 
                if (hasGoodLevel(I, v_u6, v_v6)) {
                  element = (element + 4) % 8; // turn down
                }
                else {
                  unsigned int v_u7 = u;
                  unsigned int v_v7 = v;
                  updateFreemanPosition(v_u7, v_v7, (element + 3) % 8); // diag
 
                  if (hasGoodLevel(I, v_u7, v_v7)) {
                    element = (element + 3) % 8; // turn diag right down
                  }
                  else {
                    // No neighbor with a good level
                    //
                    return false;
                  }
                }
              }
            }
          }
        }
      }
    }
  }
 
  else {
    return false;
  }
 
  return true;
}
 
void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, unsigned int &element, int &du, int &dv,
                                      float &dS, float &dMu, float &dMv, float &dMuv, float &dMu2, float &dMv2)
{
  du = 0;
  dv = 0;
  dMuv = 0;
  dMu2 = 0;
  dMv2 = 0;
 
  /*
           3  2  1
            \ | /
             \|/
         4 ------- 0
             /|\
            / | \
           5  6  7
  */
  switch (element) {
  case 0: // go right
    du = 1;
    dS = static_cast<float>(v_p);
    dMu = 0.0;
    dMv = static_cast<float>(0.5 * v_p * v_p);
    if (compute_moment) {
      dMuv = static_cast<float>(0.25 * v_p * v_p * ((2 * u_p) + 1));
      dMu2 = 0;
      dMv2 = static_cast<float>((1.0 / 3.) * v_p * v_p * v_p);
    }
    break;
 
  case 1: // go right top
    du = 1;
    dv = 1;
    dS = static_cast<float>(v_p + 0.5);
    dMu = -static_cast<float>((0.5 * u_p * (u_p + 1)) + (1.0 / 6.0));
    dMv = static_cast<float>((0.5 * v_p * (v_p + 1)) + (1.0 / 6.0));
    if (compute_moment) {
      float half_u_p = static_cast<float>(0.5 * u_p);
      dMuv = static_cast<float>((v_p * v_p * (0.25 + half_u_p)) + (v_p * ((1. / 3.) + half_u_p)) + ((1. / 6.) * u_p) + 0.125);
      dMu2 = static_cast<float>(((-1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.)) - (1. / 12.0));
      dMv2 = static_cast<float>(((1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.)) + (1. / 12.0));
    }
    break;
 
  case 2: // go top
    dv = 1;
    dS = 0.0;
    dMu = static_cast<float>(-0.5 * u_p * u_p);
    dMv = 0.0;
    if (compute_moment) {
      dMuv = 0;
      dMu2 = static_cast<float>((-1.0 / 3.) * u_p * u_p * u_p);
      dMv2 = 0;
    }
    break;
 
  case 3:
    du = -1;
    dv = 1;
    dS = static_cast<float>(-v_p - 0.5);
    dMu = -static_cast<float>((0.5 * u_p * (u_p - 1)) + (1.0 / 6.0));
    dMv = -static_cast<float>((0.5 * v_p * (v_p + 1)) + (1.0 / 6.0));
    if (compute_moment) {
      float half_u_p = static_cast<float>(0.5 * u_p);
      dMuv = static_cast<float>((((v_p * v_p * (0.25 - half_u_p)) + (v_p * ((1. / 3.) - half_u_p))) - ((1. / 6.) * u_p)) + 0.125);
      dMu2 = static_cast<float>(((-1. / 3.) * u_p * (((u_p * u_p) - (1.5 * u_p)) + 1.)) - (1. / 12.0));
      dMv2 = static_cast<float>(((-1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.)) - (1. / 12.0));
    }
    break;
 
  case 4:
    du = -1;
    dS = static_cast<float>(-v_p);
    dMv = static_cast<float>(-0.5 * v_p * v_p);
    dMu = 0.0;
    if (compute_moment) {
      dMuv = static_cast<float>(-0.25 * v_p * v_p * ((2 * u_p) - 1));
      dMu2 = 0;
      dMv2 = static_cast<float>((-1.0 / 3.) * v_p * v_p * v_p);
    }
    break;
 
  case 5:
    du = -1;
    dv = -1;
    dS = static_cast<float>(-v_p + 0.5);
    dMu = static_cast<float>((0.5 * u_p * (u_p - 1)) + (1.0 / 6.0));
    dMv = static_cast<float>(-((0.5 * v_p * (v_p - 1)) + (1.0 / 6.0)));
    if (compute_moment) {
      float half_u_p = static_cast<float>(0.5 * u_p);
      dMuv = static_cast<float>((((v_p * v_p * (0.25 - half_u_p)) - (v_p * ((1. / 3.) - half_u_p))) - ((1. / 6.) * u_p)) + 0.125);
      dMu2 = static_cast<float>(((1. / 3.) * u_p * (((u_p * u_p) - (1.5 * u_p)) + 1.)) - (1. / 12.0));
      dMv2 = static_cast<float>(((-1. / 3.) * v_p * (((v_p * v_p) - (1.5 * v_p)) + 1.)) - (1. / 12.0));
    }
    break;
 
  case 6:
    dv = -1;
    dS = 0.0;
    dMu = static_cast<float>(0.5 * u_p * u_p);
    dMv = 0.0;
    if (compute_moment) {
      dMuv = 0;
      dMu2 = static_cast<float>((1.0 / 3.) * u_p * u_p * u_p);
      dMv2 = 0;
    }
    break;
 
  case 7:
    du = 1;
    dv = -1;
    dS = static_cast<float>(v_p - 0.5);
    dMu = static_cast<float>((0.5 * u_p * (u_p + 1)) + (1.0 / 6.0));
    dMv = static_cast<float>((0.5 * v_p * (v_p - 1)) + (1.0 / 6.0));
    if (compute_moment) {
      float half_u_p = static_cast<float>(0.5 * u_p);
      dMuv = static_cast<float>(((v_p * v_p * (0.25 + half_u_p)) - (v_p * ((1. / 3.) + half_u_p))) + ((1. / 6.) * u_p) + 0.125);
      dMu2 = static_cast<float>(((1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.)) + (1. / 12.0));
      dMv2 = static_cast<float>(((1. / 3.) * v_p * (((v_p * v_p) - (1.5 * v_p)) + 1.)) - (1. / 12.0));
    }
    break;
 
  default:
    std::cout << "to complete the default" << std::endl;
  }
}
 
void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, const unsigned int &dir)
{
  switch (dir) {
  case 0:
    u += 1;
    break;
  case 1:
    u += 1;
    v += 1;
    break;
  case 2:
    v += 1;
    break;
  case 3:
    u -= 1;
    v += 1;
    break;
  case 4:
    u -= 1;
    break;
  case 5:
    u -= 1;
    v -= 1;
    break;
  case 6:
    v -= 1;
    break;
  case 7:
    u += 1;
    v -= 1;
    break;
  default:
    std::cout << "In vpDot2::updateFreemanPosition dir not identified" << std::endl;
  }
}
 
bool vpDot2::isInImage(const vpImage<unsigned char> &I) const { return isInImage(I, cog); }
 
bool vpDot2::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const
{
  unsigned int h = I.getHeight();
  unsigned int w = I.getWidth();
  double u = ip.get_u();
  double v = ip.get_v();
 
  if ((u < 0) || (u >= w)) {
    return false;
  }
  if ((v < 0) || (v >= h)) {
    return false;
  }
  return true;
}
 
bool vpDot2::isInArea(const unsigned int &u, const unsigned int &v) const
{
  unsigned int area_u_min = static_cast<unsigned int>(area.getLeft());
  unsigned int area_u_max = static_cast<unsigned int>(area.getRight());
  unsigned int area_v_min = static_cast<unsigned int>(area.getTop());
  unsigned int area_v_max = static_cast<unsigned int>(area.getBottom());
 
  if ((u < area_u_min) || (u > area_u_max)) {
    return false;
  }
  if ((v < area_v_min) || (v > area_v_max)) {
    return false;
  }
  return true;
}
 
void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight)
{
  // first get the research grid width and height Note that
  // 1/sqrt(2)=cos(pi/4). The grid squares should be small enough to be
  // contained in the dot. We gent this here if the dot is a perfect disc.
  // More accurate criterium to define the grid should be implemented if
  // necessary
  gridWidth = static_cast<unsigned int>((getWidth() * getMaxSizeSearchDistPrecision()) / sqrt(2.));
  gridHeight = static_cast<unsigned int>((getHeight() * getMaxSizeSearchDistPrecision()) / sqrt(2.0));
 
  if (gridWidth == 0) {
    gridWidth = 1;
  }
  if (gridHeight == 0) {
    gridHeight = 1;
  }
}
 
void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char> &I)
{
  int cog_u = static_cast<int>(cog.get_u());
  int cog_v = static_cast<int>(cog.get_v());
 
  unsigned int sum_value = 0;
  unsigned int nb_pixels = 0;
 
  for (unsigned int i = static_cast<unsigned int>(this->bbox_u_min); i <= static_cast<unsigned int>(this->bbox_u_max); ++i) {
    unsigned int pixel_gray = static_cast<unsigned int>(I[static_cast<unsigned int>(cog_v)][i]);
    if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
      sum_value += pixel_gray;
      ++nb_pixels;
    }
  }
  for (unsigned int i = static_cast<unsigned int>(this->bbox_v_min); i <= static_cast<unsigned int>(this->bbox_v_max); ++i) {
    unsigned char pixel_gray = I[i][static_cast<unsigned int>(cog_u)];
    if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
      sum_value += pixel_gray;
      ++nb_pixels;
    }
  }
  if (nb_pixels < 10) { // could be good to choose the min nb points from area of dot
    // add diagonals points to have enough point
    int imin, imax;
    if ((cog_u - bbox_u_min) >(cog_v - bbox_v_min)) {
      imin = cog_v - bbox_v_min;
    }
    else {
      imin = cog_u - bbox_u_min;
    }
    if ((bbox_u_max - cog_u) > (bbox_v_max - cog_v)) {
      imax = bbox_v_max - cog_v;
    }
    else {
      imax = bbox_u_max - cog_u;
    }
    for (int i = -imin; i <= imax; ++i) {
      unsigned int pixel_gray = static_cast<unsigned int>(I[static_cast<unsigned int>(cog_v + i)][static_cast<unsigned int>(cog_u + i)]);
      if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
        sum_value += pixel_gray;
        ++nb_pixels;
      }
    }
 
    if ((cog_u - bbox_u_min) > (bbox_v_max - cog_v)) {
      imin = bbox_v_max - cog_v;
    }
    else {
      imin = cog_u - bbox_u_min;
    }
    if ((bbox_u_max - cog_u) > (cog_v - bbox_v_min)) {
      imax = cog_v - bbox_v_min;
    }
    else {
      imax = bbox_u_max - cog_u;
    }
 
    for (int i = -imin; i <= imax; ++i) {
      unsigned char pixel_gray = I[static_cast<unsigned int>(cog_v - i)][static_cast<unsigned int>(cog_u + i)];
      if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) {
        sum_value += pixel_gray;
        ++nb_pixels;
      }
    }
  }
 
  if (nb_pixels == 0) {
    // should never happen
    throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No point was found"));
  }
  else {
    mean_gray_level = sum_value / nb_pixels;
  }
}
 
vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I,
                            vpColor col, bool trackDot)
{
  vpMatrix Cogs(n, 2);
  vpImagePoint cog;
  unsigned int i;
  bool fromFile = vpIoTools::checkFilename(dotFile.c_str());
  if (fromFile) {
    vpMatrix::loadMatrix(dotFile, Cogs);
    std::cout << Cogs.getRows() << " dots loaded from file " << dotFile << std::endl;
  }
 
  // test number of cogs in file
  if (Cogs.getRows() < n) {
    std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl;
    fromFile = false;
  }
 
  // read from file and tracks the dots
  if (fromFile) {
    try {
      for (i = 0; i < n; ++i) {
        cog.set_uv(Cogs[i][0], Cogs[i][1]);
        dot[i].setGraphics(true);
        dot[i].setCog(cog);
        if (trackDot) {
          dot[i].initTracking(I, cog);
          dot[i].track(I);
          vpDisplay::displayCross(I, cog, 10, col);
        }
      }
    }
    catch (...) {
      std::cout << "Cannot track dots from file" << std::endl;
      fromFile = false;
    }
    vpDisplay::flush(I);
 
    // check that dots are far away ones from the other
    for (i = 0; ((i < n) && fromFile); ++i) {
      double d = sqrt(vpMath::sqr(dot[i].getHeight()) + vpMath::sqr(dot[i].getWidth()));
      for (unsigned int j = 0; ((j < n) && fromFile); ++j) {
        if (j != i) {
          if (dot[i].getDistance(dot[j]) < d) {
            fromFile = false;
            std::cout << "Dots from file seem incoherent" << std::endl;
          }
        }
      }
    }
  }
 
  if (!fromFile) {
    vpDisplay::display(I);
    vpDisplay::flush(I);
 
    std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
    for (i = 0; i < n; ++i) {
      if (trackDot) {
        dot[i].setGraphics(true);
        dot[i].initTracking(I);
        cog = dot[i].getCog();
      }
      else {
        vpDisplay::getClick(I, cog);
        dot[i].setCog(cog);
      }
      Cogs[i][0] = cog.get_u();
      Cogs[i][1] = cog.get_v();
      vpDisplay::displayCross(I, cog, 10, col);
      vpDisplay::flush(I);
    }
  }
 
  if ((!fromFile) && (dotFile != "")) {
    vpMatrix::saveMatrix(dotFile, Cogs);
    std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl;
  }
 
  // back to non graphic mode
  for (i = 0; i < n; ++i) {
    dot[i].setGraphics(false);
  }
 
  return Cogs;
}
 
void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I,
                             std::vector<vpImagePoint> &cogs, vpImagePoint *cogStar)
{
  unsigned int i;
  // tracking
  for (i = 0; i < n; ++i) {
    dot[i].track(I);
    cogs.push_back(dot[i].getCog());
  }
  // trajectories
  unsigned int cogs_size = cogs.size();
  for (i = n; i < cogs_size; ++i) {
    vpDisplay::displayCircle(I, cogs[i], 4, vpColor::green, true);
  }
  // initial position
  for (i = 0; i < n; ++i) {
    vpDisplay::displayCircle(I, cogs[i], 4, vpColor::blue, true);
  }
  // if exists, desired position
  if (cogStar != nullptr) {
    for (i = 0; i < n; ++i) {
      vpDisplay::displayDotLine(I, cogStar[i], dot[i].getCog(), vpColor::red);
      vpDisplay::displayCircle(I, cogStar[i], 4, vpColor::red, true);
    }
  }
  vpDisplay::flush(I);
}
 
void vpDot2::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 vpDot2::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, vpDot2 &d) { return (os << "(" << d.getCog() << ")"); }