vpFeaturePointPolar.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:
 * 2D point visual feature.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeaturePointPolar.h>

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

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

// math
#include <visp3/core/vpMath.h>

#include <visp3/core/vpFeatureDisplay.h>

/*

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

*/

void vpFeaturePointPolar::init()
{
  // feature dimension
  dim_s = 2;
  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;

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

vpFeaturePointPolar::vpFeaturePointPolar() : Z(1.) { init(); }

void vpFeaturePointPolar::set_rho(const double rho)
{
  s[0] = rho;
  flags[0] = true;
}
void vpFeaturePointPolar::set_theta(const double theta)
{
  s[1] = theta;
  flags[1] = true;
}

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

void vpFeaturePointPolar::set_rhoThetaZ(const double rho, const double theta, const double Z_)
{
  set_rho(rho);
  set_theta(theta);
  set_Z(Z_);

  for (unsigned int i = 0; i < nbParameters; i++)
    flags[i] = true;
}

double vpFeaturePointPolar::get_rho() const { return s[0]; }

double vpFeaturePointPolar::get_theta() const { return s[1]; }
double vpFeaturePointPolar::get_Z() const { return this->Z; }

vpMatrix vpFeaturePointPolar::interaction(const 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 rho "
                  "was not set yet");
          break;
        case 1:
          vpTRACE("Warning !!!  The interaction matrix is computed but theta "
                  "was not set yet");
          break;
        case 2:
          vpTRACE("Warning !!!  The interaction matrix is computed but Z was "
                  "not set yet");
          break;
        default:
          vpTRACE("Problem during the reading of the variable flags");
        }
      }
    }
    resetFlags();
  }

  double rho = get_rho();
  double theta = get_theta();
  double Z_ = get_Z();

  double c_ = cos(theta);
  double s_ = sin(theta);

  double rho2 = rho * rho;

  if (fabs(rho) < 1e-6) {
    vpERROR_TRACE("rho polar coordinate of the point is null");
    std::cout << "rho = " << rho << std::endl;

    throw(vpFeatureException(vpFeatureException::badInitializationError, "rho polar coordinate of the point is null"));
  }

  if (Z_ < 0) {
    vpERROR_TRACE("Point is behind the camera ");
    std::cout << "Z = " << Z_ << std::endl;

    throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
  }

  if (fabs(Z_) < 1e-6) {
    vpERROR_TRACE("Point Z coordinates is null ");
    std::cout << "Z = " << Z_ << std::endl;

    throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
  }

  if (vpFeaturePointPolar::selectRho() & select) {
    vpMatrix Lrho(1, 6);
    Lrho = 0;

    Lrho[0][0] = -c_ / Z_;
    Lrho[0][1] = -s_ / Z_;
    Lrho[0][2] = rho / Z_;
    Lrho[0][3] = (1 + rho2) * s_;
    Lrho[0][4] = -(1 + rho2) * c_;
    Lrho[0][5] = 0;

    //     printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
    //     std::cout << "Lrho: " << Lrho << std::endl;

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

  if (vpFeaturePointPolar::selectTheta() & select) {
    vpMatrix Ltheta(1, 6);
    Ltheta = 0;

    Ltheta[0][0] = s_ / (rho * Z_);
    Ltheta[0][1] = -c_ / (rho * Z_);
    Ltheta[0][2] = 0;
    Ltheta[0][3] = c_ / rho;
    Ltheta[0][4] = s_ / rho;
    Ltheta[0][5] = -1;

    //     printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
    //     std::cout << "Ltheta: " << Ltheta << std::endl;
    L = vpMatrix::stack(L, Ltheta);
  }
  return L;
}

vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, const unsigned int select)
{
  vpColVector e(0);

  try {
    if (vpFeaturePointPolar::selectRho() & select) {
      vpColVector erho(1);
      erho[0] = s[0] - s_star[0];

      e = vpColVector::stack(e, erho);
    }

    if (vpFeaturePointPolar::selectTheta() & select) {

      //      printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
      //      s_star[1]);
      double err = s[1] - s_star[1];

      //      printf("Error: %f ", err );
      while (err < -M_PI)
        err += 2 * M_PI;
      while (err > M_PI)
        err -= 2 * M_PI;
      //     printf(" modif %f \n", err );

      vpColVector etheta(1);
      etheta[0] = err;
      e = vpColVector::stack(e, etheta);
    }
  } catch (...) {
    throw;
  }

  return e;
}

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

  std::cout << "Point:  Z=" << get_Z();
  if (vpFeaturePointPolar::selectRho() & select)
    std::cout << " rho=" << get_rho();
  if (vpFeaturePointPolar::selectTheta() & select)
    std::cout << " theta=" << get_theta();
  std::cout << std::endl;
}

void vpFeaturePointPolar::buildFrom(const double rho, const double theta, const double Z_)
{

  s[0] = rho;
  s[1] = theta;

  this->Z = Z_;

  if (Z < 0) {
    vpERROR_TRACE("Point is behind the camera ");
    std::cout << "Z = " << Z << std::endl;

    throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
  }

  if (fabs(Z) < 1e-6) {
    vpERROR_TRACE("Point Z coordinates is null ");
    std::cout << "Z = " << Z << std::endl;

    throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
  }

  for (unsigned int i = 0; i < nbParameters; i++)
    flags[i] = true;
}

void vpFeaturePointPolar::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
                                  unsigned int thickness) const
{
  try {
    double rho, theta;
    rho = get_rho();
    theta = get_theta();

    double x, y;
    x = rho * cos(theta);
    y = rho * sin(theta);

    vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
  } catch (...) {
    vpERROR_TRACE("Error caught");
    throw;
  }
}

void vpFeaturePointPolar::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
                                  unsigned int thickness) const
{
  try {
    double rho, theta;
    rho = get_rho();
    theta = get_theta();

    double x, y;
    x = rho * cos(theta);
    y = rho * sin(theta);

    vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);

  } catch (...) {
    vpERROR_TRACE("Error caught");
    throw;
  }
}

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

unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }

unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1]; }