vpDot2.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:
 * Fabien Spindler
 * Anthony Saunier
 *
 *****************************************************************************/

//#define DEBUG

#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 = m11 = m02 = m20 = m10 = m01 = 0.;
  mu11 = mu02 = mu20 = 0.;

  bbox_u_min = bbox_u_max = bbox_v_min = 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);
}

vpDot2::~vpDot2() {}

/******************************************************************************
 *
 *      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;

  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) {}

  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;

  setWidth(size);
  setHeight(size);

  try {
    track(I);
  } catch (const vpException &e) {
    // vpERROR_TRACE("Error caught") ;
    throw(e);
  }
}

void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size)
{
  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;

  setWidth(size);
  setHeight(size);

  try {
    track(I);
  } catch (const vpException &e) {
    // vpERROR_TRACE("Error caught") ;
    throw(e);
  }
}

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);

  try {
    track(I);
  } catch (const vpException &e) {
    // vpERROR_TRACE("Error caught") ;
    throw(e);
  }
}

void vpDot2::track(const vpImage<unsigned char> &I, bool canMakeTheWindowGrow)
{
  m00 = m11 = m02 = m20 = m10 = 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);

  //   vpDEBUG_TRACE(0, "Previous dot: ");
  //   vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
  //   vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
  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, searchWindowHeight = 0.0;
    // if( getWidth() == 0 || getHeight() == 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, (int)(this->cog.get_u() - searchWindowWidth / 2.0),
                     (int)(this->cog.get_v() - searchWindowHeight / 2.0), (unsigned int)searchWindowWidth,
                     (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;
  }
  //   else {
  //     // test if the found dot is valid,
  //     if( ! isValid( I, wantedDot ) ) {
  //       *this = wantedDot;
  //       vpERROR_TRACE("The found dot is invalid:",
  //            "- could be a problem of size (width or height) or "
  //            "  surface (number of pixels) which differ too much "
  //            "  to the previous one "
  //            "- or a problem of the shape which is not ellipsoid if "
  //            "  use setEllipsoidShapePrecision(double
  // ellipsoidShapePrecision) "
  //         "  which is the default case. "
  //            "  To track a non ellipsoid shape use
  // setEllipsoidShapePrecision(0)") ;
  //       throw(vpTrackingException(vpTrackingException::featureLostError,
  //                "The found dot is invalid")) ;
  //     }
  //   }

  // 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]);

  // getMeanGrayLevel(I);
  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;

  // printf("%i %i \n",gray_level_max,gray_level_min);
  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);
    // vpDisplay::flush(I);
  }
}

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::getMaxSizeSearchDistancePrecision() 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::setMaxSizeSearchDistancePrecision(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 >= (int)image_w)
    u = (int)image_w - 1;
  if (v < 0)
    v = 0;
  else if (v >= (int)image_h)
    v = (int)image_h - 1;

  if (((unsigned int)u + w) > image_w)
    w = image_w - (unsigned int)u - 1;
  if (((unsigned int)v + h) > image_h)
    h = image_h - (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);
    // vpDisplay::flush(I);
  }

#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 = NULL;
  vpDot2 tmpDot;

  unsigned int area_u_min = (unsigned int)area.getLeft();
  unsigned int area_u_max = (unsigned int)area.getRight();
  unsigned int area_v_min = (unsigned int)area.getTop();
  unsigned int area_v_max = (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 ((double)u >= vpBAD_DOT_VALUE.bbox_u_min && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
            (double)v >= vpBAD_DOT_VALUE.bbox_v_min && (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( border_u == cogBadDot.get_u() && v == cogBadDot.get_v()) {
            if ((std::fabs(border_u - cogBadDot.get_u()) <=
                 vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u())) *
                     std::numeric_limits<double>::epsilon()) &&
                (std::fabs(v - cogBadDot.get_v()) <=
                 vpMath::maximum(std::fabs((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 != NULL)
        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;

          // double epsilon = 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 != NULL)
    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 (size_precision!=0){
  {
    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 = (int)(nb_point_to_test * allowedBadPointsPercentage_);
  double step_angle = 2 * M_PI / nb_point_to_test;

  //  if (ellipsoidShape_precision != 0 && compute_moment) {
  if (std::fabs(ellipsoidShape_precision) > std::numeric_limits<double>::epsilon() && compute_moment) {
    //       std::cout << "test shape precision......................\n";
    // See F. Chaumette. Image moments: a general and useful set of features
    // for visual servoing. IEEE Trans. on Robotics, 20(4):713-723, August
    // 2004.

    // mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00
    //      = m11 - m10 * m01 / m00
    // mu20 = m20 - m00 * xg^2 = m20 - m00 * m10/m00 * m10/m00
    //      = m20 - m10^2 / m00
    // mu02 = m02 - m01^2 / m00
    // alpha = 1/2 arctan( 2 * mu11 / (mu20 - mu02) )
    //
    // a1^2 = 2 / m00 * (mu02 + mu20 + sqrt( (mu20 - mu02)^2 + 4mu11^2) )
    //
    // 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 = (unsigned int)(cog_u + innerCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta)));
      v = (unsigned int)(cog_v + innerCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta)));
      if (!this->hasGoodLevel(I, u, v)) {
//  vpTRACE("Inner circle pixel (%d, %d) has bad level for dot (%g, %g)",
//      u, v, cog_u, cog_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
        // return false;
        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; // 1.6;
    nb_bad_points = 0;
    for (double theta = 0.; theta < 2 * M_PI; theta += step_angle) {
      u = (unsigned int)(cog_u + outCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta)));
      v = (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 ((double)u < area.getLeft() || (double)u > area.getRight() || (double)v < area.getTop() ||
          (double)v > area.getBottom()) {
        continue;
      }
      if (!this->hasReverseLevel(I, u, v)) {
//  vpTRACE("Outside circle pixel (%d, %d) has bad level for dot (%g,
// %g)",        u, v, cog_u, cog_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++;
        // return false;
      }
      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 &_u, const double &_v)
{
  direction_list.clear();
  ip_edges_list.clear();

  double est_u = _u; // estimated
  double est_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((unsigned int)est_u, (unsigned int)est_v)) {
    vpDEBUG_TRACE(3,
                  "Initial pixel coordinates (%d, %d) for dot tracking are "
                  "not in the area",
                  (int)est_u, (int)est_v);
    return false;
  }

  bbox_u_min = (int)I.getWidth();
  bbox_u_max = 0;
  bbox_v_min = (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, (unsigned int)est_u, (unsigned int)est_v)) {
    vpDEBUG_TRACE(3, "Can't find a dot from pixel (%d, %d) coordinates", (int)est_u, (int)est_v);
    return false;
  }

  // find the border

  if (!findFirstBorder(I, (unsigned int)est_u, (unsigned int)est_v, this->firstBorder_u, this->firstBorder_v)) {

    vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates", (int)est_u, (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 = (int)this->firstBorder_u;
  int border_v = (int)this->firstBorder_v;

  //   vpTRACE("-----------------------------------------");
  //   vpTRACE("first border_u: %d border_v: %d dir: %d",
  //    this->firstBorder_u, this->firstBorder_v,firstDir);
  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 < (int)thickness; t++) {
        ip.set_u(border_u + t);
        ip.set_v(border_v);

        vpDisplay::displayPoint(I, ip, vpColor::red);
      }
      // vpDisplay::flush(I);
    }
#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((unsigned int)border_u, (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);

    // vpDisplay::getClick(I);

    // 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, (unsigned int)border_u, (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() != (unsigned int)border_u || getFirstBorder_v() != (unsigned int)border_v ||
            firstDir != dir) &&
           isInArea((unsigned int)border_u, (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 central moments
    if (compute_moment) {
      mu11 = m11 - tmpCenter_u * m01;
      mu02 = m02 - tmpCenter_v * m01;
      mu20 = m20 - tmpCenter_u * m10;
    }

    // check the center is in the image... never know...
    //     if( !hasGoodLevel( I, (unsigned int)tmpCenter_u,
    //             (unsigned int)tmpCenter_v ) )
    //     {
    //       vpDEBUG_TRACE(3, "The center of gravity of the dot (%g, %g) has
    //       not a good in level", tmpCenter_u, tmpCenter_v); return false;
    //     }

    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() / (getMaxSizeSearchDistancePrecision() + 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 _u = u;
    unsigned int _v = v;
    // get the point on the right of the point passed in
    updateFreemanPosition(_u, _v, (element + 2) % 8);
    if (hasGoodLevel(I, _u, _v)) {
      element = (element + 2) % 8; // turn right
    } else {
      unsigned int _u1 = u;
      unsigned int _v1 = v;
      updateFreemanPosition(_u1, _v1, (element + 1) % 8);

      if (hasGoodLevel(I, _u1, _v1)) {
        element = (element + 1) % 8; // turn diag right
      } else {
        unsigned int _u2 = u;
        unsigned int _v2 = v;
        updateFreemanPosition(_u2, _v2, element); // same direction

        if (hasGoodLevel(I, _u2, _v2)) {
          // element = element;      // keep same dir
        } else {
          unsigned int _u3 = u;
          unsigned int _v3 = v;
          updateFreemanPosition(_u3, _v3, (element + 7) % 8); // diag left

          if (hasGoodLevel(I, _u3, _v3)) {
            element = (element + 7) % 8; // turn diag left
          } else {
            unsigned int _u4 = u;
            unsigned int _v4 = v;
            updateFreemanPosition(_u4, _v4, (element + 6) % 8); // left

            if (hasGoodLevel(I, _u4, _v4)) {
              element = (element + 6) % 8; // turn left
            } else {
              unsigned int _u5 = u;
              unsigned int _v5 = v;
              updateFreemanPosition(_u5, _v5, (element + 5) % 8); // left

              if (hasGoodLevel(I, _u5, _v5)) {
                element = (element + 5) % 8; // turn diag down
              } else {
                unsigned int _u6 = u;
                unsigned int _v6 = v;
                updateFreemanPosition(_u6, _v6, (element + 4) % 8); // left

                if (hasGoodLevel(I, _u6, _v6)) {
                  element = (element + 4) % 8; // turn down
                } else {
                  unsigned int _u7 = u;
                  unsigned int _v7 = v;
                  updateFreemanPosition(_u7, _v7, (element + 3) % 8); // diag

                  if (hasGoodLevel(I, _u7, _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 = (float)v_p;
    dMu = 0.0;
    dMv = (float)(0.5 * v_p * v_p);
    if (compute_moment) {
      dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1));
      dMu2 = 0;
      dMv2 = (float)(1.0 / 3. * v_p * v_p * v_p);
    }
    break;

  case 1: // go right top
    du = 1;
    dv = 1;
    dS = (float)(v_p + 0.5);
    dMu = -(float)(0.5 * u_p * (u_p + 1) + 1.0 / 6.0);
    dMv = (float)(0.5 * v_p * (v_p + 1) + 1.0 / 6.0);
    if (compute_moment) {
      float half_u_p = (float)(0.5 * u_p);
      dMuv = (float)(v_p * v_p * (0.25 + half_u_p) + v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125);
      dMu2 = (float)(-1. / 3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1. / 12.0);
      dMv2 = (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 = (float)(-0.5 * u_p * u_p);
    dMv = 0.0;
    if (compute_moment) {
      dMuv = 0;
      dMu2 = (float)(-1.0 / 3. * u_p * u_p * u_p);
      dMv2 = 0;
    }
    break;

  case 3:
    du = -1;
    dv = 1;
    dS = (float)(-v_p - 0.5);
    dMu = -(float)(0.5 * u_p * (u_p - 1) + 1.0 / 6.0);
    dMv = -(float)(0.5 * v_p * (v_p + 1) + 1.0 / 6.0);
    if (compute_moment) {
      float half_u_p = (float)(0.5 * u_p);
      dMuv = (float)(v_p * v_p * (0.25 - half_u_p) + v_p * (1. / 3. - half_u_p) - 1. / 6. * u_p + 0.125);
      dMu2 = (float)(-1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1. / 12.0);
      dMv2 = (float)(-1. / 3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1. / 12.0);
    }
    break;

  case 4:
    du = -1;
    dS = (float)(-v_p);
    dMv = (float)(-0.5 * v_p * v_p);
    dMu = 0.0;
    if (compute_moment) {
      dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1));
      dMu2 = 0;
      dMv2 = (float)(-1.0 / 3. * v_p * v_p * v_p);
    }
    break;

  case 5:
    du = -1;
    dv = -1;
    dS = (float)(-v_p + 0.5);
    dMu = (float)(0.5 * u_p * (u_p - 1) + 1.0 / 6.0);
    dMv = (float)(-(0.5 * v_p * (v_p - 1) + 1.0 / 6.0));
    if (compute_moment) {
      float half_u_p = (float)(0.5 * u_p);
      dMuv = (float)(v_p * v_p * (0.25 - half_u_p) - v_p * (1. / 3. - half_u_p) - 1. / 6. * u_p + 0.125);
      dMu2 = (float)(1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1. / 12.0);
      dMv2 = (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 = (float)(0.5 * u_p * u_p);
    dMv = 0.0;
    if (compute_moment) {
      dMuv = 0;
      dMu2 = (float)(1.0 / 3. * u_p * u_p * u_p);
      dMv2 = 0;
    }
    break;

  case 7:
    du = 1;
    dv = -1;
    dS = (float)(v_p - 0.5);
    dMu = (float)(0.5 * u_p * (u_p + 1) + 1.0 / 6.0);
    dMv = (float)(0.5 * v_p * (v_p - 1) + 1.0 / 6.0);
    if (compute_moment) {
      float half_u_p = (float)(0.5 * u_p);
      dMuv = (float)(v_p * v_p * (0.25 + half_u_p) - v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125);
      dMu2 = (float)(1. / 3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1. / 12.0);
      dMv2 = (float)(1. / 3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1. / 12.0);
    }
    break;
  }
}

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

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 = (unsigned int)area.getLeft();
  unsigned int area_u_max = (unsigned int)area.getRight();
  unsigned int area_v_min = (unsigned int)area.getTop();
  unsigned int area_v_max = (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 = (unsigned int)(getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.));
  gridHeight = (unsigned int)(getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0));

  if (gridWidth == 0)
    gridWidth = 1;
  if (gridHeight == 0)
    gridHeight = 1;
}

void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char> &I)
{
  int cog_u = (int)cog.get_u();
  int cog_v = (int)cog.get_v();

  unsigned int sum_value = 0;
  unsigned int nb_pixels = 0;

  for (unsigned int i = (unsigned int)this->bbox_u_min; i <= (unsigned int)this->bbox_u_max; i++) {
    unsigned int pixel_gray = (unsigned int)I[(unsigned int)cog_v][i];
    if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()) {
      sum_value += pixel_gray;
      nb_pixels++;
    }
  }
  for (unsigned int i = (unsigned int)this->bbox_v_min; i <= (unsigned int)this->bbox_v_max; i++) {
    unsigned char pixel_gray = I[i][(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 = (unsigned int)I[(unsigned int)(cog_v + i)][(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[(unsigned int)(cog_v - i)][(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
  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 != NULL)
    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;

  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;

  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() << ")"); }