vpDot.cpp

/****************************************************************************
 *
 * $Id: vpDot.cpp 4649 2014-02-07 14:57:11Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Track a white dot.
 *
 * Authors:
 * Eric Marchand
 * Fabien Spindler
 * Aurelien Yol
 *
 *****************************************************************************/

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

#include <visp/vpDot.h>

#include <visp/vpDisplay.h>
#include <visp/vpColor.h>

// exception handling
#include <visp/vpTrackingException.h>
#include <vector>

#ifdef VISP_USE_MSVC
#  pragma comment(linker, "/STACK:256000000") // Increase max recursion depth
#endif

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

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

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

  compute_moment = false ;
  graphics = false ;
  thickness = 1;
  maxDotSizePercentage = 0.25 ; // 25 % of the image size
  
  mean_gray_level = 0;
  gray_level_min = 128;
  gray_level_max = 255;
  grayLevelPrecision = 0.85;
  gamma = 1.5 ;

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

  connexityType = CONNEXITY_4;

  u_min = u_max = v_min = v_max = 0;

  gray_level_out = 0;
  nbMaxPoint = 0;
}

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

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

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

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

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

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

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

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

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

  return *this ;
}

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

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

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

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

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

  // Test if we are in the image
  if ( (u >= width) || (v >= height) )
  {
    //std::cout << "out of bound" << std::endl;
    return false;
  }
  
  if(checkTab[u + v*I.getWidth()])
    return true;
  
  vpImagePoint ip;
  ip.set_u(u);
  ip.set_v(v);  
  
  if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
  {
    checkTab[v*I.getWidth() + u] = true;

    ip_connexities_list.push_back(ip);

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

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

      throw(vpTrackingException(vpTrackingException::featureLostError,
                "Dot to big")) ;
    }

    // Bounding box update
    if (u < this->u_min) this->u_min = u;
    if (u > this->u_max) this->u_max = u;
    if (v < this->v_min) this->v_min = v;
    if (v > this->v_max) this->v_max = v;
    
    // Mean value of the dot intensities
    mean_value = (mean_value *(n-1) + I[v][u]) / n;
    if (compute_moment==true)
    {
      m00++ ;
      m10 += u ;
      m01 += v ;
      m11 += (u*v) ;
      m20 += u*u ;
      m02 += v*v ;
    }
  }
  else
  {
    //std::cout << "not in" << std::endl;
    return false;
  }
  
  bool edge = false;
  
  //if((int)u-1 >= 0)
  if(u >= 1)
    if(!checkTab[u-1 + v*I.getWidth()])
      if(!connexe(I,u-1,v, mean_value,u_cog,v_cog, n, checkTab))
    edge = true;
  
  if(u+1 < I.getWidth())
    if(!checkTab[u+1+v*I.getWidth()])
      if(!connexe(I,u+1,v,mean_value,u_cog, v_cog, n, checkTab))
    edge = true;
      
  if(v >= 1)
    if(!checkTab[u+(v-1)*I.getWidth()])
      if(!connexe(I,u, v-1,mean_value,u_cog, v_cog, n, checkTab))
    edge = true;
      
  if(v+1 < I.getHeight())
    if(!checkTab[u+(v+1)*I.getWidth()])
      if(!connexe(I,u,v+1,mean_value,u_cog, v_cog, n, checkTab))
    edge = true;
  
  if (connexityType == CONNEXITY_8) {
    if(v >= 1 && u >= 1)
      if(!checkTab[u-1+(v-1)*I.getWidth()])
        if(!connexe(I,u-1,v-1,mean_value,u_cog, v_cog, n, checkTab))
          edge = true;
      
    if(v >= 1 && u+1 < I.getWidth())
      if(!checkTab[u+1+(v-1)*I.getWidth()])
        if(!connexe(I,u+1,v-1,mean_value,u_cog, v_cog, n, checkTab))
          edge = true;
      
    if(v+1 < I.getHeight() && u >= 1)
      if(!checkTab[u-1+(v+1)*I.getWidth()])
        if(!connexe(I,u-1,v+1,mean_value, u_cog, v_cog, n, checkTab))
          edge = true;
      
    if(v+1 < I.getHeight() && u+1 < I.getWidth())
      if(!checkTab[u+1+(v+1)*I.getWidth()])
        if(!connexe(I,u+1,v+1,mean_value,u_cog, v_cog, n, checkTab))
          edge = true;
   }
  
  if(edge){
    ip_edges_list.push_back(ip);
    if (graphics==true)
    {
      vpImagePoint ip_(ip);
      for(unsigned int t=0; t<thickness; t++) {
        ip_.set_u(ip.get_u() + t);
        vpDisplay::displayPoint(I, ip_, vpColor::red) ;
      }
      //vpDisplay::flush(I);
    }
  }
  
  return true;
}

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

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

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

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

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

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

    // Spiral search from the center to find the nearest dot
    while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
      for (k=1; k <= right; k++) if(sol==false) {
    u_cog = 0 ;
    v_cog = 0 ;
    ip_connexities_list.clear() ;
    ip_edges_list.clear();
    
    this->mean_gray_level = 0 ;
    if ( connexe(I, (unsigned int)u_+k, (unsigned int)(v_),mean_gray_level, u_cog, v_cog, npoint) ) {
      sol = true; u = u_+k; v = v_;
    }
      }
      u_ += k;
      right += 2;

      for (k=1; k <= botom; k++) if (sol==false) {
    u_cog = 0 ;
    v_cog = 0 ;
    ip_connexities_list.clear() ;
    ip_edges_list.clear();
    
    this->mean_gray_level = 0 ;
    
    if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),mean_gray_level, u_cog, v_cog, npoint) ) {
      sol = true; u = u_; v = v_+k;
    }
      }
      v_ += k;
      botom += 2;

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

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

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

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

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

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

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

  u = u_cog ;
  v = v_cog ;

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

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

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

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

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

   throw(vpTrackingException(vpTrackingException::featureLostError,
                  "Dot to big")) ;
  }  
}

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

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

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

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

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

  try {
    track( I );
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
}

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

  cog = ip ;

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

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

  if(Ip - (1 - grayLevelPrecision)<0){
    gray_level_min = 0 ;
  }
  else{
    gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
    if (gray_level_min > 255)
      gray_level_min = 255;
  }
  gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
  if (gray_level_max > 255)
    gray_level_max = 255;
  try {
    track( I );
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
}

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

  cog = ip ;

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

  try {
    track( I );
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
}


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

    COG( I, u, v ) ;

    this->cog.set_u( u );
    this->cog.set_v( v );
    
    if (compute_moment==true)
    {
      mu11 = m11 - u*m01;
      mu02 = m02 - v*m01;
      mu20 = m20 - u*m10;
    }

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

  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
}

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

  ip = this->cog;
}

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

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

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

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

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

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

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

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