vpFeaturePointPolar.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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.
 *
*****************************************************************************/
 
#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
 
*/
 
BEGIN_VISP_NAMESPACE
void vpFeaturePointPolar::init()
{
  // feature dimension
  dim_s = 2;
  nbParameters = 3;
 
  // memory allocation
  s.resize(dim_s);
  if (flags == nullptr)
    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(double rho)
{
  s[0] = rho;
  flags[0] = true;
}
void vpFeaturePointPolar::set_theta(double theta)
{
  s[1] = theta;
  flags[1] = true;
}
 
void vpFeaturePointPolar::set_Z(double Z_)
{
  this->Z = Z_;
  flags[2] = true;
}
 
void vpFeaturePointPolar::set_rhoThetaZ(double rho, double theta, 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(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, 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(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;
}
 
vpFeaturePointPolar &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;
  }
  return *this;
}
 
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]; }
END_VISP_NAMESPACE