vpGenericFeature.cpp

/****************************************************************************
 *
 * $Id: vpGenericFeature.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:
 * Generic feature (used to create new feature not implemented in ViSP).
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <visp/vpGenericFeature.h>


// Exception
#include <visp/vpException.h>
#include <visp/vpMatrixException.h>
#include <visp/vpFeatureException.h>

// Debug trace
#include <visp/vpDebug.h>



vpGenericFeature::~vpGenericFeature()
{

}

void vpGenericFeature::init()
{
  s = 0 ;
}


vpGenericFeature::vpGenericFeature()
  : L(), err(), errorStatus(errorNotInitalized)
{
  /*
  vpERROR_TRACE("You are not allow to use this constructor ") ;
  vpERROR_TRACE("Please, use  vpGenericFeature::vpGenericFeature(int _dim) "
          "constructor") ;
  vpERROR_TRACE("And provide the dimension of the visual feature ") ;
  throw(vpException(vpException::cannotUseConstructorError,
                 "You are not allow to use this constructor ")) ;
  */
}


vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s)
  : L(), err(), errorStatus(errorNotInitalized)
{
  this->dim_s = dimension_gen_s ;
  s.resize(dimension_gen_s) ;
}

void
vpGenericFeature::setError(const vpColVector &error_vector)
{
  if (error_vector.getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between error dimension"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between error dimension"
                 "and feature dimension"));

  }
  errorStatus = errorInitialized ;
  err = error_vector ;
}


vpColVector
vpGenericFeature::error(const vpBasicFeature &s_star,
            const unsigned int select)
{
  if (s_star.get_s().getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between s* dimension "
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between s* dimension "
                 "and feature dimension"));

  }

  vpColVector e(0) ;

  try
  {
    if (errorStatus == errorHasToBeUpdated)
    {
      vpERROR_TRACE("Error has no been updated since last iteration"
          "you should have used vpGenericFeature::setError"
          "in you visual servoing loop") ;
      throw(vpFeatureException(vpFeatureException::badErrorVectorError,
                   "Error has no been updated since last iteration"));
    }
    else
      if (errorStatus == errorInitialized)
      {
    vpDEBUG_TRACE(25,"Error init: e=e.");
    errorStatus = errorHasToBeUpdated ;
    for (unsigned int i=0 ; i < dim_s ; i++)
      if (FEATURE_LINE[i] & select )
      {
        vpColVector ex(1) ;
        ex[i] = err[i] ;

        e = vpMatrix::stackMatrices(e,ex) ;
      }
      }
      else
      {
    vpDEBUG_TRACE(25,"Error not init: e=s-s*.");

    for (unsigned int i=0 ; i < dim_s ; i++)
      if (FEATURE_LINE[i] & select )
      {
        vpColVector ex(1) ;
        ex[0] = s[i] - s_star[i] ;

        e = vpMatrix::stackMatrices(e,ex) ;
      }

      }
  }
  catch(vpMatrixException me)
  {
    vpERROR_TRACE("caught a Matric related error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
  return e ;

}



vpColVector
vpGenericFeature::error( const unsigned int select)
{
  vpColVector e(0) ;

  try
  {
    if (errorStatus == errorHasToBeUpdated)
    {
      vpERROR_TRACE("Error has no been updated since last iteration"
          "you should have used vpGenericFeature::setError"
          "in you visual servoing loop") ;
      throw(vpFeatureException(vpFeatureException::badErrorVectorError,
                   "Error has no been updated since last iteration"));
    }
    else
      if (errorStatus == errorInitialized)
      {
    errorStatus = errorHasToBeUpdated ;
    for (unsigned int i=0 ; i < dim_s ; i++)
      if (FEATURE_LINE[i] & select )
      {
        vpColVector ex(1) ;
        ex[i] = err[i] ;

        e = vpMatrix::stackMatrices(e,ex) ;
      }
      }
      else
      {

    for (unsigned int i=0 ; i < dim_s ; i++)
      if (FEATURE_LINE[i] & select )
      {
        vpColVector ex(1) ;
        ex[i] = s[i]  ;

        e = vpMatrix::stackMatrices(e,ex) ;
      }

      }
  }
  catch(vpMatrixException me)
  {
    vpERROR_TRACE("caught a Matric related error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }

  return e ;

}


vpMatrix
vpGenericFeature::interaction(const unsigned int select)
{
  if (L.getRows() == 0)
  {
    std::cout << "interaction matrix " << L << std::endl ;
    vpERROR_TRACE("Interaction has not been initialized");
    std::cout << "A possible reason (may be) is that you have set" << std::endl ;
    std::cout << "the interaction matrix for s and compute a control " << std::endl ;
    std::cout << "with Ls=s* (default) or vice versa" << std::endl ;

    throw(vpFeatureException(vpFeatureException::notInitializedError,
                 "size mismatch between s* dimension "
                 "and feature dimension"));

  }

  vpMatrix Ls ;

  Ls.resize(0,6) ;

  for (unsigned int i=0 ; i < dim_s ; i++)
    if (FEATURE_LINE[i] & select )
    {
      vpMatrix Lx(1,6) ; Lx = 0;

      for (int j=0 ; j < 6 ; j++)
    Lx[0][j] = L[i][j] ;

      Ls = vpMatrix::stackMatrices(Ls,Lx) ;
    }

  return Ls ;
}


void
vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
{
  if (L_.getRows() != dim_s)
  {
    std::cout << L_.getRows() <<"  " << dim_s << std::endl ;;
    vpERROR_TRACE("size mismatch between interaction matrix size "
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between interaction matrix size "
                 "and feature dimension"));
  }

  this->L = L_ ;
}

void
vpGenericFeature::set_s(const vpColVector &s_vector)
{

  if (s_vector.getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between s dimension"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between s dimension"
                 "and feature dimension"));
  }
  this->s = s_vector ;
}


void
vpGenericFeature::get_s(vpColVector &s_vector) const
{
  if (s_vector.getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between s dimension"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between s dimension"
                 "and feature dimension"));
  }
  s_vector = this->s ;
}


void
vpGenericFeature::set_s(const double s0, const double s1, const double s2)
{

  if (3 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between  number of parameters"
                 "and feature dimension"));

  }
  s[0] = s0 ; s[1] = s1 ; s[2] = s2 ;
}


void
vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
{

  if (3 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between  number of parameters"
                 "and feature dimension"));

  }
  s0 = s[0] ; s1 = s[1] ; s2 = s[2] ;
}


void
vpGenericFeature::set_s(const double s0, const double s1)
{

  if (2 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between  number of parameters"
                 "and feature dimension"));

  }
  s[0] = s0 ; s[1] = s1 ;
}


void
vpGenericFeature::get_s(double &s0, double &s1) const
{

  if (2 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between  number of parameters"
                 "and feature dimension"));

  }
  s0 = s[0] ; s1 = s[1] ;
}


void
vpGenericFeature::set_s(const double s0)
{

  if (1 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between  number of parameters"
                 "and feature dimension"));

  }
  s[0] = s0 ;
}


void
vpGenericFeature::get_s(double &s0) const
{

  if (1 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
        "and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
                 "size mismatch between  number of parameters"
                 "and feature dimension"));

  }
  s0 = s[0];
}


void
vpGenericFeature::print(const unsigned int select) const
{

  std::cout <<"Generic Feature: "  ;
  for (unsigned int i=0 ; i < dim_s ; i++)
    if (FEATURE_LINE[i] & select )
    {
      std::cout << " s["<<i << "]=" << s[i] ;
    }

  std::cout <<std::endl ;
}

vpGenericFeature *vpGenericFeature::duplicate() const
{
  vpGenericFeature *feature= new vpGenericFeature(dim_s) ;

  vpTRACE("dims = %d",dim_s) ;
  return feature ;
}

void
vpGenericFeature::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
vpGenericFeature::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
  }
}

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