vpDot2.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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.
 */
 
#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>
 
BEGIN_VISP_NAMESPACE
 
/******************************************************************************
 *
 *      CONSTRUCTORS AND DESTRUCTORS
 *
*****************************************************************************/
void vpDot2::init()
{
  const unsigned int val_max = 255;
  const unsigned int val_median = 128;
  m_cog.set_u(0);
  m_cog.set_v(0);
 
  m_width = 0;
  m_height = 0;
  m_surface = 0;
  m_mean_gray_level = 0;
  m_gray_level_min = val_median;
  m_gray_level_max = val_max;
  m_grayLevelPrecision = 0.80;
  m_gamma = 1.5;
 
  m_sizePrecision = 0.65;
  m_ellipsoidShapePrecision = 0.65;
  m_maxSizeSearchDistPrecision = 0.65;
  setEllipsoidBadPointsPercentage();
  m00 = 0.;
  m11 = 0.;
  m02 = 0.;
  m20 = 0.;
  m10 = 0.;
  m01 = 0.;
  mu11 = 0.;
  mu02 = 0.;
  mu20 = 0.;
 
  m_bbox_u_min = 0;
  m_bbox_u_max = 0;
  m_bbox_v_min = 0;
  m_bbox_v_max = 0;
 
  m_firstBorder_u = 0;
  m_firstBorder_v = 0;
 
  m_compute_moment = false;
  m_graphics = false;
  m_thickness = 1;
}
 
vpDot2::vpDot2()
  : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(), m_width(0), m_height(0),
  m_surface(0), m_mean_gray_level(0), m_grayLevelPrecision(0.8), m_gamma(1.5),
  m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), m_maxSizeSearchDistPrecision(0.65),
  m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), m_compute_moment(false), m_graphics(false),
  m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), m_firstBorder_u(0), m_firstBorder_v()
{
  const unsigned int val_max = 255;
  const unsigned int val_median = 128;
  m_gray_level_min = val_median;
  m_gray_level_max = val_max;
}
 
vpDot2::vpDot2(const vpImagePoint &ip)
  : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(ip), m_width(0), m_height(0),
  m_surface(0), m_mean_gray_level(0), m_grayLevelPrecision(0.8), m_gamma(1.5),
  m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), m_maxSizeSearchDistPrecision(0.65),
  m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), m_compute_moment(false), m_graphics(false),
  m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), m_firstBorder_u(0), m_firstBorder_v()
{
  const unsigned int val_max = 255;
  const unsigned int val_median = 128;
  m_gray_level_min = val_median;
  m_gray_level_max = val_max;
}
 
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.), m_cog(),
  m_width(0), m_height(0), m_surface(0), m_mean_gray_level(0),
  m_grayLevelPrecision(0.8), m_gamma(1.5), m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65),
  m_maxSizeSearchDistPrecision(0.65), m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(),
  m_compute_moment(false), m_graphics(false), m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0),
  m_firstBorder_u(0), m_firstBorder_v()
{
  const unsigned int val_max = 255;
  const unsigned int val_median = 128;
  m_gray_level_min = val_median;
  m_gray_level_max = val_max;
  *this = twinDot;
}
 
vpDot2 &vpDot2::operator=(const vpDot2 &twinDot)
{
  m_cog = twinDot.m_cog;
 
  m_width = twinDot.m_width;
  m_height = twinDot.m_height;
  m_surface = twinDot.m_surface;
  m_gray_level_min = twinDot.m_gray_level_min;
  m_gray_level_max = twinDot.m_gray_level_max;
  m_mean_gray_level = twinDot.m_mean_gray_level;
  m_grayLevelPrecision = twinDot.m_grayLevelPrecision;
  m_gamma = twinDot.m_gamma;
 
  m_sizePrecision = twinDot.m_sizePrecision;
  m_ellipsoidShapePrecision = twinDot.m_ellipsoidShapePrecision;
  m_maxSizeSearchDistPrecision = twinDot.m_maxSizeSearchDistPrecision;
  m_allowedBadPointsPercentage = twinDot.m_allowedBadPointsPercentage;
  m_area = twinDot.m_area;
 
  m_direction_list = twinDot.m_direction_list;
  m_ip_edges_list = twinDot.m_ip_edges_list;
 
  m_compute_moment = twinDot.m_compute_moment;
  m_graphics = twinDot.m_graphics;
  m_thickness = twinDot.m_thickness;
 
  m_bbox_u_min = twinDot.m_bbox_u_min;
  m_bbox_u_max = twinDot.m_bbox_u_max;
  m_bbox_v_min = twinDot.m_bbox_v_min;
  m_bbox_v_max = twinDot.m_bbox_v_max;
 
  m_firstBorder_u = twinDot.m_firstBorder_u;
  m_firstBorder_v = twinDot.m_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
{
  const unsigned int val_3 = 3;
  const unsigned int val_8 = 8;
  vpDisplay::displayCross(I, m_cog, (val_3 * t) + val_8, color, t);
  std::list<vpImagePoint>::const_iterator it;
 
  std::list<vpImagePoint>::const_iterator ip_edges_list_end = m_ip_edges_list.end();
  for (it = m_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, m_cog) != true) {
    // block empty waiting user interaction
  }
 
  unsigned int i = static_cast<unsigned int>(m_cog.get_i());
  unsigned int j = static_cast<unsigned int>(m_cog.get_j());
  const unsigned int val_max = 255;
 
  double Ip = pow(static_cast<double>(I[i][j]) / val_max, 1 / m_gamma);
 
  if ((Ip - (1 - m_grayLevelPrecision)) < 0) {
    m_gray_level_min = 0;
  }
  else {
    m_gray_level_min = static_cast<unsigned int>(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma));
    if (m_gray_level_min > val_max) {
      m_gray_level_min = val_max;
    }
  }
  m_gray_level_max = static_cast<unsigned int>(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma));
  if (m_gray_level_max > val_max) {
    m_gray_level_max = val_max;
  }
 
  setWidth(size);
  setHeight(size);
 
  track(I);
}
 
void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size)
{
  m_cog = ip;
 
  unsigned int i = static_cast<unsigned int>(m_cog.get_i());
  unsigned int j = static_cast<unsigned int>(m_cog.get_j());
  const unsigned int val_max = 255;
 
  double Ip = pow(static_cast<double>(I[i][j]) / val_max, 1 / m_gamma);
 
  if ((Ip - (1 - m_grayLevelPrecision)) < 0) {
    m_gray_level_min = 0;
  }
  else {
    m_gray_level_min = static_cast<unsigned int>(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma));
    if (m_gray_level_min > val_max) {
      m_gray_level_min = val_max;
    }
  }
  m_gray_level_max = static_cast<unsigned int>(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma));
  if (m_gray_level_max > val_max) {
    m_gray_level_max = val_max;
  }
 
  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)
{
  m_cog = ip;
 
  m_gray_level_min = gray_lvl_min;
  m_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, m_cog.get_u(), m_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) {
    // 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) {
      const unsigned int val_5 = 5;
      searchWindowWidth = getWidth() * val_5;
      searchWindowHeight = getHeight() * val_5;
    }
    else {
      searchWindowWidth = getWidth();
      searchWindowHeight = getHeight();
    }
 
    std::list<vpDot2> candidates;
    searchDotsInArea(I, static_cast<int>(m_cog.get_u() - (searchWindowWidth / 2.0)),
                     static_cast<int>(m_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()) {
      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
    m_bbox_u_min = movingDot.m_bbox_u_min;
    m_bbox_u_max = movingDot.m_bbox_u_max;
    m_bbox_v_min = movingDot.m_bbox_v_min;
    m_bbox_v_max = movingDot.m_bbox_v_max;
  }
 
  // if this dot is partially out of the image, return an error tracking.
  if (!isInImage(I)) {
    throw(vpTrackingException(vpTrackingException::featureLostError,
                              "The center of gravity of the dot is not in the image"));
  }
 
  const unsigned int val_max = 255;
  double Ip = pow(getMeanGrayLevel() / val_max, 1 / m_gamma);
  // printf("current value of gray level center : %i\n", I[v][u]);
 
  // get Mean Gray Level of I
  if ((Ip - (1 - m_grayLevelPrecision)) < 0) {
    m_gray_level_min = 0;
  }
  else {
    m_gray_level_min = static_cast<unsigned int>(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma));
    if (m_gray_level_min > val_max) {
      m_gray_level_min = val_max;
    }
  }
  m_gray_level_max = static_cast<unsigned int>(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma));
  if (m_gray_level_max > val_max) {
    m_gray_level_max = val_max;
  }
 
  if (m_graphics) {
    // display a red cross at the center of gravity's location in the image.
    const unsigned int val_3 = 3;
    const unsigned int val_8 = 8;
    vpDisplay::displayCross(I, m_cog, (val_3 * m_thickness) + val_8, vpColor::red, m_thickness);
  }
}
 
void vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip, bool canMakeTheWindowGrow)
{
  track(I, canMakeTheWindowGrow);
 
  ip = m_cog;
}
 
 
double vpDot2::getWidth() const { return m_width; }
 
double vpDot2::getHeight() const { return m_height; }
 
double vpDot2::getArea() const { return fabs(m_surface); }
 
double vpDot2::getGrayLevelPrecision() const { return m_grayLevelPrecision; }
 
double vpDot2::getSizePrecision() const { return m_sizePrecision; }
 
double vpDot2::getEllipsoidShapePrecision() const { return m_ellipsoidShapePrecision; }
 
double vpDot2::getMaxSizeSearchDistPrecision() const { return m_maxSizeSearchDistPrecision; }
 
double vpDot2::getDistance(const vpDot2 &distantDot) const
{
  vpImagePoint cogDistantDot = distantDot.getCog();
  double diff_u = m_cog.get_u() - cogDistantDot.get_u();
  double diff_v = m_cog.get_v() - cogDistantDot.get_v();
  return sqrt((diff_u * diff_u) + (diff_v * diff_v));
}
 
 
void vpDot2::setWidth(const double &w) { m_width = w; }
 
void vpDot2::setHeight(const double &h) { m_height = h; }
 
void vpDot2::setArea(const double &a) { m_surface = a; }
 
void vpDot2::setGrayLevelPrecision(const double &precision)
{
  double epsilon = 0.05;
  if (m_grayLevelPrecision < epsilon) {
    m_grayLevelPrecision = epsilon;
  }
  else if (m_grayLevelPrecision > 1) {
    m_grayLevelPrecision = 1.0;
  }
  else {
    m_grayLevelPrecision = precision;
  }
}
void vpDot2::setSizePrecision(const double &precision)
{
  if (m_sizePrecision < 0) {
    m_sizePrecision = 0;
  }
  else if (m_sizePrecision > 1) {
    m_sizePrecision = 1.0;
  }
  else {
    m_sizePrecision = precision;
  }
}
 
void vpDot2::setEllipsoidShapePrecision(const double &precision)
{
 
  if (m_ellipsoidShapePrecision < 0) {
    m_ellipsoidShapePrecision = 0;
  }
  else if (m_ellipsoidShapePrecision > 1) {
    m_ellipsoidShapePrecision = 1.0;
  }
  else {
    m_ellipsoidShapePrecision = precision;
  }
}
 
void vpDot2::setMaxSizeSearchDistPrecision(const double &precision)
{
  double epsilon = 0.05;
  if (m_maxSizeSearchDistPrecision < epsilon) {
    m_maxSizeSearchDistPrecision = epsilon;
  }
  else if (m_maxSizeSearchDistPrecision > 1) {
    m_maxSizeSearchDistPrecision = 1.0;
  }
  else {
    m_maxSizeSearchDistPrecision = 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;
  }
 
  m_area.setRect(u, v, w, h);
}
 
void vpDot2::setArea(const vpRect &area) { m_area = area; }
 
 
void vpDot2::searchDotsInArea(const vpImage<unsigned char> &I, std::list<vpDot2> &niceDots)
{
  searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
}
 
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) {
#ifdef DEBUG
        printf("Bad width > for dot (%g, %g)\n", m_cog.get_u(), m_cog.get_v());
#endif
        return false;
      }
 
      if ((getWidth() < (wantedDot.getWidth() / (size_precision + epsilon))) == false) {
#ifdef DEBUG
        printf("Bad width %g > %g for dot (%g, %g)\n", getWidth(), wantedDot.getWidth() / (size_precision + epsilon),
               m_cog.get_u(), m_cog.get_v());
#endif
        return false;
      }
 
      if ((((wantedDot.getHeight() * size_precision) - epsilon) < getHeight()) == false) {
#ifdef DEBUG
        printf("Bad height %g > %g for dot (%g, %g)\n", wantedDot.getHeight() * size_precision - epsilon, getHeight(),
               m_cog.get_u(), m_cog.get_v());
#endif
        return false;
      }
 
      if ((getHeight() < (wantedDot.getHeight() / (size_precision + epsilon))) == false) {
#ifdef DEBUG
        printf("Bad height %g > %g for dot (%g, %g)\n", getHeight(), wantedDot.getHeight() / (size_precision + epsilon),
               m_cog.get_u(), m_cog.get_v());
#endif
        return false;
      }
 
      if ((((wantedDot.getArea() * (size_precision * size_precision)) - epsilon) < getArea()) == false) {
#ifdef DEBUG
        printf("Bad surface %g > %g for dot (%g, %g)\n",
               wantedDot.getArea() * (size_precision * size_precision) - epsilon, getArea(), m_cog.get_u(), m_cog.get_v());
#endif
        return false;
      }
 
      if ((getArea() < (wantedDot.getArea() / ((size_precision * size_precision) + epsilon))) == false) {
#ifdef DEBUG
        printf("Bad surface %g < %g for dot (%g, %g)\n", getArea(),
               wantedDot.getArea() / (size_precision * size_precision + epsilon), m_cog.get_u(), m_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 * m_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()) && m_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 = m_cog.get_u();
    double cog_v = m_cog.get_v();
    double val_2 = 2;
 
    vpImagePoint ip;
    nb_bad_points = 0;
    for (double theta = 0.; theta < (val_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], m_gray_level_min, m_gray_level_max);
#endif
        ++nb_bad_points;
      }
      if (m_graphics) {
        for (unsigned int t = 0; t < m_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 < (val_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) < m_area.getLeft()) ||
          (static_cast<double>(u) > m_area.getRight()) ||
          (static_cast<double>(v) < m_area.getTop()) ||
          (static_cast<double>(v) > m_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], m_gray_level_min, m_gray_level_max);
#endif
          ++nb_bad_points;
        }
        if (m_graphics) {
          for (unsigned int t = 0; t < m_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] >= m_gray_level_min) && (I[v][u] <= m_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] < m_gray_level_min) || (I[v][u] > m_gray_level_max)) {
    return true;
  }
  else {
    return false;
  }
}
 
vpDot2 *vpDot2::getInstance() { return new vpDot2(); }
 
 
/******************************************************************************
 *
 *      PRIVATE METHODS
 *
 ******************************************************************************/
 
bool vpDot2::computeParameters(const vpImage<unsigned char> &I, const double &v_u, const double &v_v)
{
  m_direction_list.clear();
  m_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 = m_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 = m_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))) {
    return false;
  }
 
  m_bbox_u_min = static_cast<int>(I.getWidth());
  m_bbox_u_max = 0;
  m_bbox_v_min = static_cast<int>(I.getHeight());
  m_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))) {
    return false;
  }
 
  // find the border
 
  if (!findFirstBorder(I, static_cast<unsigned int>(est_u), static_cast<unsigned int>(est_v), m_firstBorder_u, m_firstBorder_v)) {
    return false;
  }
 
  unsigned int dir = 6;
 
  // Determine the first element of the Freeman chain
  computeFreemanChainElement(I, m_firstBorder_u, m_firstBorder_v, dir);
  unsigned int firstDir = dir;
 
  // if we are now out of the image, return an error tracking
  if (!isInArea(m_firstBorder_u, m_firstBorder_v)) {
    return false;
  }
 
  // store the new direction and dot border coordinates.
  m_direction_list.push_back(dir);
  vpImagePoint ip;
  ip.set_u(m_firstBorder_u);
  ip.set_v(m_firstBorder_v);
 
  m_ip_edges_list.push_back(ip);
 
  int border_u = static_cast<int>(m_firstBorder_u);
  int border_v = static_cast<int>(m_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 (m_graphics) {
      for (int t = 0; t < static_cast<int>(m_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 (m_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))) {
      // Can Occur on a single pixel dot located on the top border
      return false;
    }
 
    // store the new direction and dot border coordinates.
 
    m_direction_list.push_back(dir);
 
    ip.set_u(border_u);
    ip.set_v(border_v);
    m_ip_edges_list.push_back(ip);
 
    // update the extreme point of the dot.
    if (border_v < m_bbox_v_min) {
      m_bbox_v_min = border_v;
    }
    if (border_v > m_bbox_v_max) {
      m_bbox_v_max = border_v;
    }
    if (border_u < m_bbox_u_min) {
      m_bbox_u_min = border_u;
    }
    if (border_u > m_bbox_u_max) {
      m_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) {
      return false;
    }
  } 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()))) {
    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 (m_compute_moment) {
      mu11 = m11 - (tmpCenter_u * m01);
      mu02 = m02 - (tmpCenter_v * m01);
      mu20 = m20 - (tmpCenter_u * m10);
    }
 
    m_cog.set_u(tmpCenter_u);
    m_cog.set_v(tmpCenter_v);
  }
 
  m_width = (m_bbox_u_max - m_bbox_u_min) + 1;
  m_height = (m_bbox_v_max - m_bbox_v_min) + 1;
  m_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 something 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: " << m_gray_level_min << " " << m_gray_level_max << std::endl;
#endif
  while (hasGoodLevel(I, border_u + 1, border_v) && (border_u < m_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))) {
      return false;
    }
#ifdef DEBUG
    vpDisplay::displayPoint(I, static_cast<int>(border_v), static_cast<int>(border_u) + 1, vpColor::green);
    vpDisplay::flush(I);
#endif
 
    ++border_u;
  }
  return true;
}
 
bool vpDot2::isInImage(const vpImage<unsigned char> &I) const { return isInImage(I, m_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>(m_area.getLeft());
  unsigned int area_u_max = static_cast<unsigned int>(m_area.getRight());
  unsigned int area_v_min = static_cast<unsigned int>(m_area.getTop());
  unsigned int area_v_max = static_cast<unsigned int>(m_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>(m_cog.get_u());
  int cog_v = static_cast<int>(m_cog.get_v());
 
  unsigned int sum_value = 0;
  unsigned int nb_pixels = 0;
 
  for (unsigned int i = static_cast<unsigned int>(m_bbox_u_min); i <= static_cast<unsigned int>(m_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>(m_bbox_v_min); i <= static_cast<unsigned int>(m_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;
    }
  }
  const unsigned int nb_min_pixels = 10;
  if (nb_pixels < nb_min_pixels) { // 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 - m_bbox_u_min) >(cog_v - m_bbox_v_min)) {
      imin = cog_v - m_bbox_v_min;
    }
    else {
      imin = cog_u - m_bbox_u_min;
    }
    if ((m_bbox_u_max - cog_u) > (m_bbox_v_max - cog_v)) {
      imax = m_bbox_v_max - cog_v;
    }
    else {
      imax = m_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 - m_bbox_u_min) > (m_bbox_v_max - cog_v)) {
      imin = m_bbox_v_max - cog_v;
    }
    else {
      imin = cog_u - m_bbox_u_min;
    }
    if ((m_bbox_u_max - cog_u) > (cog_v - m_bbox_v_min)) {
      imax = cog_v - m_bbox_v_min;
    }
    else {
      imax = m_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 {
    m_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 {
      const unsigned int cross_size = 10;
      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, cross_size, 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
    i = 0;
    while ((i < n) && fromFile) {
      double d = sqrt(vpMath::sqr(dot[i].getHeight()) + vpMath::sqr(dot[i].getWidth()));
      unsigned int j = 0;
      while ((j < n) && fromFile) {
        if (j != i) {
          if (dot[i].getDistance(dot[j]) < d) {
            fromFile = false;
            std::cout << "Dots from file seem incoherent" << std::endl;
          }
        }
        ++j;
      }
      ++i;
    }
  }
 
  if (!fromFile) {
    vpDisplay::display(I);
    vpDisplay::flush(I);
 
    std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
    const unsigned int cross_size = 10;
    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, cross_size, 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)
{
  // tracking
  for (unsigned int i = 0; i < n; ++i) {
    dot[i].track(I);
    cogs.push_back(dot[i].getCog());
  }
  // trajectories
  unsigned int cogs_size = static_cast<unsigned int>(cogs.size());
  for (unsigned int i = n; i < cogs_size; ++i) {
    const unsigned int circle_size = 4;
    vpDisplay::displayCircle(I, cogs[i], circle_size, vpColor::green, true);
  }
  // initial position
  for (unsigned int i = 0; i < n; ++i) {
    const unsigned int circle_size = 4;
    vpDisplay::displayCircle(I, cogs[i], circle_size, vpColor::blue, true);
  }
  // if exists, desired position
  if (cogStar != nullptr) {
    const unsigned int circle_size = 10;
    for (unsigned int i = 0; i < n; ++i) {
      vpDisplay::displayDotLine(I, cogStar[i], dot[i].getCog(), vpColor::red);
      vpDisplay::displayCircle(I, cogStar[i], circle_size, 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)
{
  const unsigned int val_3 = 3;
  const unsigned int val_8 = 8;
 
  vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_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)
{
  const unsigned int val_3 = 3;
  const unsigned int val_8 = 8;
  vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_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() << ")"); }
 
END_VISP_NAMESPACE