vpMe.cpp

/****************************************************************************
 *
 * $Id: vpMe.cpp 5060 2014-12-12 18:31:03Z 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:
 * Moving edges.
 *
 * Authors:
 * Eric Marchand
 * Andrew Comport
 *
 *****************************************************************************/

#include <visp/vpMe.h>
#include <visp/vpColVector.h>
#include <visp/vpMath.h>
#include <stdlib.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS


struct point
{
  double x ;
  double y ;
} ;

struct droite
{
  double a ;
  double b ;
  double c ;
} ;







template <class Type>
inline void
permute(Type & a, Type & b)
{
  Type t = a;
  a = b;
  b = t;
}

static droite
droite_cartesienne(point P, point Q)
{
  droite PQ;

  PQ.a = P.y-Q.y;
  PQ.b = Q.x-P.x;
  PQ.c = Q.y*P.x - Q.x*P.y;

  return(PQ);
}



static point
point_intersection(droite D1, droite D2)
{
 point  I;
 double det;  // determinant des 2 vect.normaux

 det = (D1.a*D2.b - D2.a*D1.b); // interdit D1,D2 paralleles
 I.x = (D2.c*D1.b - D1.c*D2.b)/det;
 I.y = (D1.c*D2.a - D2.c*D1.a)/det;

 return(I);
}

static void
recale(point & P,
             double Xmin, double Ymin, double Xmax, double Ymax)
{
  if(vpMath::equal(P.x,Xmin))
    P.x=Xmin; // a peu pres => exactement !
  if(vpMath::equal(P.x,Xmax))
        P.x=Xmax;

  if(vpMath::equal(P.y,Ymin))
        P.y=Ymin;
  if(vpMath::equal(P.y,Ymax))
        P.y=Ymax;
}


static void
permute(point &A,  point &B)
{
  point C ;

  if (A.x>B.x) // fonction sans doute a tester...
  {
    C = A ;
    A = B ;
    B = C ;
  }
}


// vrai si partie visible
static bool
clipping  (point  A, point B,
                     double Xmin, double Ymin, double Xmax, double Ymax,
           point  & Ac , point & Bc )// resultat: A,B clippes
{
 droite AB, D[4];
 D[0].a = 1;     D[0].b = 0;     D[0].c = -Xmin;
 D[1].a = 1;     D[1].b = 0;     D[1].c = -Xmax;
 D[2].a = 0;     D[2].b = 1;     D[2].c = -Ymin;
 D[3].a = 0;     D[3].b = 1;     D[3].c = -Ymax;

 point P[2];
 P[0]=A; P[1]=B;
 int code_P[2], // codes de P[n]
 i, bit_i,        // i -> (0000100...)
 n;

 AB = droite_cartesienne(A,B);

 for ( ; ; )                               // 2 sorties directes internes
 {
   // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
   // ================================================
   for(n=0; n<2; n++)
   {
     code_P[n] = 0000;

     if( P[n].x < Xmin )
             code_P[n] |= 1;         // positionne bit0
     if( P[n].x > Xmax )
             code_P[n] |= 2;         //    ..      bit1
     if( P[n].y < Ymin )
             code_P[n] |= 4;         //    ..      bit2
     if( P[n].y > Ymax )
             code_P[n] |= 8;         //    ..      bit3
   }


   // 2 CAS OU L'ON PEUT CONCLURE => sortie
   // =====================================
   if((code_P[0] | code_P[1])==0000)  // Aucun bit a 1
        /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
        { if(P[0].x < P[1].x) // Rend le couple de points
                { Ac=P[0];  Bc=P[1]; }  //  clippes (ordonnes selon
          else  { Ac=P[1];  Bc=P[0]; }  //  leur abscisse x)
        */
    {
            Ac=P[0];  Bc=P[1];
      if(vpMath::equal(Ac.x,Bc.x) && vpMath::equal(Ac.y,Bc.y))
        return(false);    // AB = 1 point = invisible
      else
        return(true);    // Partie de AB clippee visible!
    }

   if((code_P[0] & code_P[1])!=0000)  // au moins 1 bit commun
   {
     return(false);  // AB completement invisible!
   }


   // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
   //   - clippe le point P[n] qui sort de la fenêtre (coupe Droite i)
   //   - reboucle avec le nouveau couple de points
   // ================================================================
   if(code_P[0] != 0000)
   {
            n=0;   // c'est P[0] qu'on clippera
      for(i=0,bit_i=1;  !(code_P[0] & bit_i);  i++,bit_i<<=1){;}
   }
   else
     {
            n=1;   // c'est P[1] qu'on clippera
      for(i=0,bit_i=1;  !(code_P[1] & bit_i);  i++,bit_i<<=1){;}
   }

   P[n] = point_intersection(AB,D[i]); // clippe le point concerne


   // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
   // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
   // =======================================================
   recale(P[n], Xmin,Ymin,Xmax,Ymax);

 }

}


// calcule la surface relative des 2 portions definies
// par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax
// Rem : P,Q tries sur x, et donc seulement 6 cas
static double
S_relative(point P, point Q,
       double Xmin, double Ymin, double Xmax, double Ymax)
{

  if(Q.x < P.x)         // tri le couple de points
    permute(P,Q);  //  selon leur abscisse x



  recale(P, Xmin,Ymin,Xmax,Ymax);  // permet des calculs de S_relative
  recale(Q, Xmin,Ymin,Xmax,Ymax);  //  moins approximatifs.

  //if(P.x==Xmin && Q.x==Xmax)
  if((std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()) 
     && 
     (std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon()))
    return( fabs(Ymax+Ymin-P.y-Q.y) );

  //if( (P.y==Ymin && Q.y==Ymax) ||
  //  (Q.y==Ymin && P.y==Ymax))
  if( ( (std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon()) 
    && 
    (std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) ) 
      || 
      ( (std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
    &&
    (std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) ) )
    return( fabs(Xmax+Xmin-P.x-Q.x) );

  //if( P.x==Xmin && Q.y==Ymax )
  if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
      &&
      std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon() )
    return( 1-(Ymax-P.y)*(Q.x-Xmin) );
  //if( P.x==Xmin && Q.y==Ymin )
  if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
      && 
      std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon() )
    return( 1-(P.y-Ymin)*(Q.x-Xmin) );
  //if( P.y==Ymin && Q.x==Xmax )
  if( std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon()
      && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon() )
    return( 1-(Xmax-P.x)*(Q.y-Ymin) );
  //if( P.y==Ymax && Q.x==Xmax )
  if( std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()
      && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon())
    return( 1-(Xmax-P.x)*(Ymax-Q.y) );


  printf("utils_ecm: ERREUR dans S_relative (%f,%f) (%f,%f) %f %f %f %f\n",
     P.x,P.y,Q.x,Q.y,Xmin,Ymin,Xmax,Ymax);
  exit(-1); // DEBUG Stoppe net l'execution  
}


static void
calcul_masques(vpColVector &angle, // definitions des angles theta
           unsigned int n,             // taille masques (PAIRE ou IMPAIRE Ok)
         vpMatrix *M)        // resultat M[theta](n,n)
{
  // Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)

  unsigned int i_theta,  // indice (boucle sur les masques)
       i,j;      // indices de boucle sur M(i,j)
  double X,Y,   // point correspondant/centre du masque
    theta, cos_theta, sin_theta, tan_theta,
    moitie = ((double)n)/2.0; // moitie REELLE du masque
  point P1,Q1,P,Q;  // clippe Droite(theta) P1,Q1 -> P,Q
  int    sgn;       // signe de M(i,j)
  double v;         // ponderation de M(i,j)

 unsigned int nb_theta = angle.getRows() ;

 for(i_theta=0; i_theta<nb_theta; i_theta++)
 {
   theta = M_PI/180*angle[i_theta]; // indice i -> theta(i) en radians
                                                                //  angle[] dans [0,180[
   cos_theta = cos(theta);        // vecteur directeur de l'ECM
   sin_theta = sin(theta);        //  associe au masque

   // PRE-CALCULE 2 POINTS DE D(theta) BIEN EN DEHORS DU MASQUE
   // =========================================================
   //if( angle[i_theta]==90 )                     // => tan(theta) infinie !
   if( std::fabs(angle[i_theta]-90) <= vpMath::maximum(std::fabs(angle[i_theta]), 90.)*std::numeric_limits<double>::epsilon() )                     // => tan(theta) infinie !
   {
     P1.x=0; P1.y=-(int)n;
     Q1.x=0; Q1.y= n;
   }
   else
   {
     tan_theta = sin_theta/cos_theta;       // pente de la droite D(theta)
     P1.x=-(int)n; P1.y=tan_theta*(-(int)n);
     Q1.x=n;  Q1.y=tan_theta*n;
   }

   // CALCULE MASQUE M(theta)
   // ======================
   M[i_theta].resize(n,n);  // allocation (si necessaire)

   for(i=0,Y=-moitie+0.5 ;   i<n  ; i++,Y++)
   {
     for(j=0,X=-moitie+0.5 ;   j<n  ; j++,X++)
     {
       // produit vectoriel dir_droite*(X,Y)
       sgn = vpMath::sign(cos_theta*Y - sin_theta*X);

       // Resultat = P,Q
       if( clipping(P1,Q1, X-0.5,Y-0.5,X+0.5,Y+0.5, P,Q) )
       {
     // v dans [0,1]
     v=S_relative(P,Q, X-0.5,Y-0.5,X+0.5,Y+0.5);
       }
       else
     v=1; // PQ ne coupe pas le pixel(i,j)

       M[i_theta][i][j] = vpMath::round(100*sgn*v);

       // 2 chiffres significatifs
       // M(i,j) sans incorporer le coef a
     }
   }
 }

}

#endif

void
vpMe::initMask()
{

  if (mask != NULL)
    delete [] mask;

  mask = new vpMatrix[n_mask] ;

  vpColVector angle(n_mask) ;

  unsigned int angle_pas ;
  angle_pas = 180 / n_mask ;

  unsigned int k =0 ;
  for (unsigned int i = 0 ; /* i < 180, */ k < n_mask ; i += angle_pas)
    angle[k++] = i ;

  calcul_masques(angle, mask_size, mask ) ;

}



void
vpMe::print( )
{

  std::cout<< std::endl ;
  std::cout<<"Moving edges settings " <<std::endl  ;
  std::cout<< std::endl ;
  std::cout<<" Size of the convolution masks...."<<mask_size<<"x"<<mask_size<<" pixels"<<std::endl ;
  std::cout<<" Number of masks.................."<<n_mask<<"        "<<std::endl ;
  std::cout<<" Query range +/- J................"<<range<<" pixels  "<<std::endl ;
  std::cout<<" Likelihood test ratio............"<<threshold<<std::endl ;
  std::cout<<" Contrast tolerance +/-..........."<< mu1 * 100<<"% and "<<mu2 * 100<<"%     "<<std::endl ;
  std::cout<<" Sample step......................"<<sample_step<<" pixels"<<std::endl ;
  std::cout<<" Strip............................"<<strip<<" pixels  "<<std::endl ;
  std::cout<<" Min_Samplestep..................."<<min_samplestep<<" pixels  "<<std::endl ;
  
  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
  std::cout<<" Aberration......................."<<aberration<< std::endl ;
  std::cout<<" init_aberration.................."<<init_aberration<<std::endl ;
  #endif
}

vpMe::vpMe()
  : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
    range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
    n_mask(180), strip(2), mask(NULL)
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
  , aberration(2), init_aberration(5.)
#endif
{
  //ntotal_sample = 0; // not sure that it is used
  //points_to_track = 500; // not sure that it is used
  anglestep = (180 / n_mask) ;
    
  initMask() ;
}

vpMe::vpMe(const vpMe &me)
  : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
    range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
    n_mask(180), strip(2), mask(NULL)
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
  , aberration(2), init_aberration(5.)
#endif
{
  *this = me;
}

const
vpMe& vpMe::operator=(const vpMe &me)
{
  if (mask != NULL) {
    delete [] mask; 
    mask = NULL ; 
  }
  threshold = me.threshold ;
  mu1 = me.mu1 ;
  mu2 = me.mu2 ;
  min_samplestep = me.min_samplestep ;
  anglestep = me.anglestep ;
  mask_size = me.mask_size ;
  n_mask = me.n_mask ;
  mask_sign = me.mask_sign ;
  range = me.range ;
  sample_step = me.sample_step ;
  ntotal_sample = me.ntotal_sample;
  points_to_track = me.points_to_track;
  strip = me.strip ;
  
  #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
  aberration = me.aberration ;
  init_aberration = me.init_aberration;
  #endif
  
  initMask() ;
  return *this;
}

vpMe::~vpMe()
{
  if (mask != NULL)
  {
    delete [] mask ;
    mask = NULL;
  }
}


void
vpMe::setMaskNumber(const unsigned int &n)
{
  n_mask = n ;
  anglestep = 180 / n_mask ;
  initMask() ;
}

void
vpMe::setMaskSize(const unsigned int &s)
{
  mask_size = s  ;
  initMask() ;
}