vpFeatureLuminance.cpp

#include <visp/vpMatrix.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpDisplay.h>
#include <visp/vpPixelMeterConversion.h>
#include <visp/vpImageConvert.h>
#include <visp/vpImageFilter.h>
#include <visp/vpException.h>

#include <visp/vpFeatureLuminance.h>


void
vpFeatureLuminance::init()
{
    if (flags == NULL)
      flags = new bool[nbParameters];
    for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;

    //default value Z (1 meters)
    Z = 1;

    firstTimeIn =0 ;

    nbr = nbc = 0;
}


void
vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
{
  init() ;

  nbr = _nbr ;
  nbc = _nbc ;

  if((nbr < 2*bord) || (nbc < 2*bord)){
    throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
  }

  // number of feature = nb column x nb lines in the images
  dim_s = (nbr-2*bord)*(nbc-2*bord) ;

  s.resize(dim_s) ;
  
  if (pixInfo != NULL)
    delete [] pixInfo;

  pixInfo = new vpLuminance[dim_s] ;
  
  Z = _Z ;
}

vpFeatureLuminance::vpFeatureLuminance()
  : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
{
    nbParameters = 1;
    dim_s = 0 ;
    flags = NULL;

    init() ;
}

vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance& f)
  : vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
{
  *this = f;
}

vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance& f)
{
  Z = f.Z;
  nbr = f.nbr;
  nbc = f.nbc;
  bord = f.bord;
  firstTimeIn = f.firstTimeIn;
  cam = f.cam;
  if (pixInfo)
    delete [] pixInfo;
  pixInfo = new vpLuminance[dim_s] ;
  for(unsigned int i=0; i< dim_s; i++)
    pixInfo[i] = f.pixInfo[i];
  return (*this);
}

vpFeatureLuminance::~vpFeatureLuminance() 
{
  if (pixInfo != NULL) delete [] pixInfo ;
  if (flags != NULL) delete [] flags;
}



void
vpFeatureLuminance::set_Z(const double Z_)
{
    this->Z = Z_ ;
    flags[0] = true;
}


double
vpFeatureLuminance::get_Z() const
{
    return Z ;
}


void
vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam) 
{
  cam = _cam ;
}


void
vpFeatureLuminance::buildFrom(vpImage<unsigned char> &I)
{
  unsigned int l = 0;
  double Ix,Iy ;

  double px = cam.get_px() ;
  double py = cam.get_py() ;


  if (firstTimeIn==0)
    { 
      firstTimeIn=1 ;
      l =0 ;
      for (unsigned int i=bord; i < nbr-bord ; i++)
    {
      //   cout << i << endl ;
      for (unsigned int j = bord ; j < nbc-bord; j++)
        {   double x=0,y=0;
          vpPixelMeterConversion::convertPoint(cam,
                           j,i,
                           x,y)  ;
        
          pixInfo[l].x = x;
          pixInfo[l].y = y;

          pixInfo[l].Z   = Z ;

          l++;
        }
    }
    }

  l= 0 ;
  for (unsigned int i=bord; i < nbr-bord ; i++)
    {
      //   cout << i << endl ;
      for (unsigned int j = bord ; j < nbc-bord; j++)
    {
      // cout << dim_s <<" " <<l <<"  " <<i << "  " << j <<endl ;
          Ix =  px * vpImageFilter::derivativeFilterX(I,i,j) ;
      Iy =  py * vpImageFilter::derivativeFilterY(I,i,j) ;
      
      // Calcul de Z
      
      pixInfo[l].I  =  I[i][j] ;
      s[l]  =  I[i][j] ;
      pixInfo[l].Ix  = Ix;
      pixInfo[l].Iy  = Iy;
      
      l++;
    }
    }

}




void
vpFeatureLuminance::interaction(vpMatrix &L)
{
  double x,y,Ix,Iy,Zinv;

  L.resize(dim_s,6) ;

  for(unsigned int m = 0; m< L.getRows(); m++)
    {
      Ix = pixInfo[m].Ix;
      Iy = pixInfo[m].Iy;

      x = pixInfo[m].x ;
      y = pixInfo[m].y ;
      Zinv =  1 / pixInfo[m].Z;

      {
    L[m][0] = Ix * Zinv;
    L[m][1] = Iy * Zinv;
    L[m][2] = -(x*Ix+y*Iy)*Zinv;
    L[m][3] = -Ix*x*y-(1+y*y)*Iy;
    L[m][4] = (1+x*x)*Ix + Iy*x*y;
    L[m][5]  = Iy*x-Ix*y;
      }
    }
}

vpMatrix  vpFeatureLuminance::interaction(const unsigned int /* select */)
{
  /* static */ vpMatrix L  ; // warning C4640: 'L' : construction of local static object is not thread-safe
  interaction(L) ;
  return L ;
}


void
vpFeatureLuminance::error(const vpBasicFeature &s_star,
              vpColVector &e)
{
  e.resize(dim_s) ;

  for (unsigned int i =0 ; i < dim_s ; i++)
    {
      e[i] = s[i] - s_star[i] ;
    }
}



vpColVector
vpFeatureLuminance::error(const vpBasicFeature &s_star,
              const unsigned int /* select */)
{
  /* static */ vpColVector e ; // warning C4640: 'e' : construction of local static object is not thread-safe
  
  error(s_star, e) ;
  
  return e ;

}




void
vpFeatureLuminance::print(const unsigned int /* select */) const
{
  static int firsttime =0 ;

  if (firsttime==0)
  {
    firsttime=1 ;
    vpERROR_TRACE("not implemented") ;
    // Do not throw and error since it is not subject
    // to produce a failure
  }
 }



void
vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
                            const vpImage<unsigned char> & /* I */,
                            const vpColor &/* color */,
                            unsigned int /* thickness */) const
{
 static int firsttime =0 ;

  if (firsttime==0)
  {
    firsttime=1 ;
    vpERROR_TRACE("not implemented") ;
    // Do not throw and error since it is not subject
    // to produce a failure
  }
}

void
vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
                            const vpImage<vpRGBa> & /* I */,
                            const vpColor &/* color */,
                            unsigned int /* thickness */) const
{
  static int firsttime =0 ;

  if (firsttime==0)
  {
    firsttime=1 ;
    vpERROR_TRACE("not implemented") ;
    // Do not throw and error since it is not subject
    // to produce a failure
  }
}


vpFeatureLuminance *vpFeatureLuminance::duplicate() const
{
  vpFeatureLuminance *feature = new vpFeatureLuminance ;
  return feature ;
}


/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */