vpFeatureThetaU.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:
 * ThetaU visual feature.
 *
 * Authors:
 * Eric Marchand
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpMath.h>
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureThetaU.h>

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

// Debug trace
#include <visp3/core/vpDebug.h>

/*

attributes and members directly related to the vpBasicFeature needs
other functionalities are useful but not mandatory

*/

void vpFeatureThetaU::init()
{
  // feature dimension
  dim_s = 3;
  nbParameters = 3;

  // memory allocation
  s.resize(dim_s);
  if (flags == NULL)
    flags = new bool[nbParameters];
  for (unsigned int i = 0; i < nbParameters; i++)
    flags[i] = false;
}

vpFeatureThetaU::vpFeatureThetaU() : rotation(vpFeatureThetaU::cdRc)
{
  // vpTRACE("0x%x", this);
  init();
}

vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
  // vpTRACE("0x%x", this);
  init();

  // kind of rotation representation
  rotation = r;
}

vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu, vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
  init();

  buildFrom(tu);
}

vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R, vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
  init();

  vpThetaUVector tu(R);
  buildFrom(tu);
}

vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M, vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
  init();
  vpRotationMatrix R;
  M.extract(R);
  vpThetaUVector tu(R);
  buildFrom(tu);
}

void vpFeatureThetaU::buildFrom(vpThetaUVector &tu)
{
  s[0] = tu[0];
  s[1] = tu[1];
  s[2] = tu[2];
  for (unsigned int i = 0; i < nbParameters; i++)
    flags[i] = true;
}

void vpFeatureThetaU::buildFrom(const vpRotationMatrix &R)
{
  vpThetaUVector tu(R);
  buildFrom(tu);
}

void vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
  vpRotationMatrix R;
  M.extract(R);
  vpThetaUVector tu(R);
  buildFrom(tu);
}

void vpFeatureThetaU::setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r) { rotation = r; }

void vpFeatureThetaU::set_TUx(double tu_x)
{
  s[0] = tu_x;
  flags[0] = true;
}
void vpFeatureThetaU::set_TUy(double tu_y)
{
  s[1] = tu_y;
  flags[1] = true;
}
void vpFeatureThetaU::set_TUz(double tu_z)
{
  s[2] = tu_z;
  flags[2] = true;
}

vpFeatureThetaU::vpFeatureThetaURotationRepresentationType vpFeatureThetaU::getFeatureThetaURotationType() const
{
  return rotation;
}

double vpFeatureThetaU::get_TUx() const { return s[0]; }

double vpFeatureThetaU::get_TUy() const { return s[1]; }

double vpFeatureThetaU::get_TUz() const { return s[2]; }

vpMatrix vpFeatureThetaU::interaction(unsigned int select)
{

  vpMatrix L;
  L.resize(0, 6);

  if (deallocate == vpBasicFeature::user) {
    for (unsigned int i = 0; i < nbParameters; i++) {
      if (flags[i] == false) {
        switch (i) {
        case 0:
          vpTRACE("Warning !!!  The interaction matrix is computed but Tu_x "
                  "was not set yet");
          break;
        case 1:
          vpTRACE("Warning !!!  The interaction matrix is computed but Tu_y "
                  "was not set yet");
          break;
        case 2:
          vpTRACE("Warning !!!  The interaction matrix is computed but Tu_z "
                  "was not set yet");
          break;
        default:
          vpTRACE("Problem during the reading of the variable flags");
        }
      }
    }
    resetFlags();
  }

  // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
  vpColVector u(3);
  for (unsigned int i = 0; i < 3; i++) {
    u[i] = s[i] / 2.0;
  }

  vpMatrix Lw(3, 3);
  Lw = vpColVector::skew(u); /* [theta/2  u]_x */

  vpMatrix U2(3, 3);
  U2.eye();

  double theta = sqrt(s.sumSquare());
  if (theta >= 1e-6) {
    for (unsigned int i = 0; i < 3; i++)
      u[i] = s[i] / theta;

    vpMatrix skew_u;
    skew_u = vpColVector::skew(u);
    U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * skew_u * skew_u;
  }

  if (rotation == cdRc) {
    Lw += U2;
  } else {
    Lw -= U2;
  }

  // This version is a simplification
  if (vpFeatureThetaU::selectTUx() & select) {
    vpMatrix Lx(1, 6);

    Lx[0][0] = 0;
    Lx[0][1] = 0;
    Lx[0][2] = 0;
    for (int i = 0; i < 3; i++)
      Lx[0][i + 3] = Lw[0][i];

    L = vpMatrix::stack(L, Lx);
  }

  if (vpFeatureThetaU::selectTUy() & select) {
    vpMatrix Ly(1, 6);

    Ly[0][0] = 0;
    Ly[0][1] = 0;
    Ly[0][2] = 0;
    for (int i = 0; i < 3; i++)
      Ly[0][i + 3] = Lw[1][i];

    L = vpMatrix::stack(L, Ly);
  }

  if (vpFeatureThetaU::selectTUz() & select) {
    vpMatrix Lz(1, 6);

    Lz[0][0] = 0;
    Lz[0][1] = 0;
    Lz[0][2] = 0;
    for (int i = 0; i < 3; i++)
      Lz[0][i + 3] = Lw[2][i];

    L = vpMatrix::stack(L, Lz);
  }

  return L;
}

vpColVector vpFeatureThetaU::error(const vpBasicFeature &s_star, unsigned int select)
{

  if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
    vpERROR_TRACE("s* should be zero ! ");
    throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
  }

  vpColVector e(0);

  if (vpFeatureThetaU::selectTUx() & select) {
    vpColVector ex(1);
    ex[0] = s[0];
    e = vpColVector::stack(e, ex);
  }

  if (vpFeatureThetaU::selectTUy() & select) {
    vpColVector ey(1);
    ey[0] = s[1];
    e = vpColVector::stack(e, ey);
  }

  if (vpFeatureThetaU::selectTUz() & select) {
    vpColVector ez(1);
    ez[0] = s[2];
    e = vpColVector::stack(e, ez);
  }
  return e;
}

void vpFeatureThetaU::print(unsigned int select) const
{
  std::cout << "ThetaU:";
  if (vpFeatureThetaU::selectTUx() & select) {
    std::cout << " tux=" << s[0];
  }
  if (vpFeatureThetaU::selectTUy() & select) {
    std::cout << " tuy=" << s[1];
  }
  if (vpFeatureThetaU::selectTUz() & select) {
    std::cout << " tuz=" << s[2];
  }
  std::cout << std::endl;
}

vpFeatureThetaU *vpFeatureThetaU::duplicate() const
{
  vpFeatureThetaU *feature;
  if (rotation == cdRc)
    feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc);
  else // if (rotation == cRcd
    feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd);

  return feature;
}

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

unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; }
unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1]; }
unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2]; }