vpGenericFeature.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 http://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:
 * Generic feature (used to create new feature not implemented in ViSP).
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/visual_features/vpGenericFeature.h>

// Exception
#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureException.h>

// Debug trace
#include <visp3/core/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, 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 = vpColVector::stack(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 = vpColVector::stack(e, ex);
        }
    }
  } catch (...) {
    throw;
  }
  return e;
}

vpColVector vpGenericFeature::error(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 = vpColVector::stack(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 = vpColVector::stack(e, ex);
        }
    }
  } catch (...) {
    throw;
  }

  return e;
}

vpMatrix vpGenericFeature::interaction(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::stack(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(double s0, double s1, 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(double s0, 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(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(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:
 */