vpFeatureLine.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 line visual feature.
 *
*****************************************************************************/
 
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureLine.h>
 
// Exception
#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureException.h>
 
// Debug trace
#include <visp3/core/vpDebug.h>
 
// simple math function (round)
#include <visp3/core/vpMath.h>
 
// Display Issue
 
// Meter/pixel conversion
#include <visp3/core/vpCameraParameters.h>
 
// Color / image / display
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.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 vpFeatureLine::init()
{
  // feature dimension
  dim_s = 2;
  nbParameters = 6;
 
  // memory allocation
  //  x cos(theta) + y sin(theta) - rho = 0
  // s[0] = rho
  // s[1] = theta
  s.resize(dim_s);
  if (flags == nullptr)
    flags = new bool[nbParameters];
  for (unsigned int i = 0; i < nbParameters; i++)
    flags[i] = false;
 
  A = B = C = D = 0.0;
}
 
vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
 
void vpFeatureLine::setRhoTheta(double rho, double theta)
{
  s[0] = rho;
  s[1] = theta;
  for (int i = 0; i < 2; i++)
    flags[i] = true;
}
 
void vpFeatureLine::setABCD(double A_, double B_, double C_, double D_)
{
  this->A = A_;
  this->B = B_;
  this->C = C_;
  this->D = D_;
  for (unsigned int i = 2; i < nbParameters; i++)
    flags[i] = true;
}
 
vpMatrix vpFeatureLine::interaction(unsigned int select)
{
  vpMatrix L;
 
  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 A was "
                  "not set yet");
          break;
        case 3:
          vpTRACE("Warning !!!  The interaction matrix is computed but B was "
                  "not set yet");
          break;
        case 4:
          vpTRACE("Warning !!!  The interaction matrix is computed but C was "
                  "not set yet");
          break;
        case 5:
          vpTRACE("Warning !!!  The interaction matrix is computed but D was "
                  "not set yet");
          break;
        default:
          vpTRACE("Problem during the reading of the variable flags");
        }
      }
    }
    resetFlags();
  }
  double rho = s[0];
  double theta = s[1];
 
  double co = cos(theta);
  double si = sin(theta);
 
  if (fabs(D) < 1e-6) {
    vpERROR_TRACE("Incorrect plane  coordinates D is null, D = %f", D);
 
    throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane  coordinates D"));
  }
 
  double lambda_theta = (A * si - B * co) / D;
  double lambda_rho = (C + rho * A * co + rho * B * si) / D;
 
  if (vpFeatureLine::selectRho() & select) {
    vpMatrix Lrho(1, 6);
 
    Lrho[0][0] = co * lambda_rho;
    Lrho[0][1] = si * lambda_rho;
    Lrho[0][2] = -rho * lambda_rho;
    Lrho[0][3] = si * (1.0 + rho * rho);
    Lrho[0][4] = -co * (1.0 + rho * rho);
    Lrho[0][5] = 0.0;
 
    L.stack(Lrho);
  }
 
  if (vpFeatureLine::selectTheta() & select) {
    vpMatrix Ltheta(1, 6);
 
    Ltheta[0][0] = co * lambda_theta;
    Ltheta[0][1] = si * lambda_theta;
    Ltheta[0][2] = -rho * lambda_theta;
    Ltheta[0][3] = -rho * co;
    Ltheta[0][4] = -rho * si;
    Ltheta[0][5] = -1.0;
 
    L.stack(Ltheta);
  }
  return L;
}
 
vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, unsigned int select)
{
  vpColVector e(0);
 
  try {
    if (vpFeatureLine::selectRho() & select) {
      vpColVector erho(1);
      erho[0] = s[0] - s_star[0];
 
      e = vpColVector::stack(e, erho);
    }
 
    if (vpFeatureLine::selectTheta() & select) {
 
      double err = s[1] - s_star[1];
      while (err < -M_PI)
        err += 2 * M_PI;
      while (err > M_PI)
        err -= 2 * M_PI;
 
      vpColVector etheta(1);
      etheta[0] = err;
      e = vpColVector::stack(e, etheta);
    }
  }
  catch (...) {
    throw;
  }
 
  return e;
}
 
void vpFeatureLine::print(unsigned int select) const
{
 
  std::cout << "Line:\t  " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
  ;
  if (vpFeatureLine::selectRho() & select)
    std::cout << "     \trho=" << s[0];
  if (vpFeatureLine::selectTheta() & select)
    std::cout << "     \ttheta=" << s[1];
  std::cout << std::endl;
}
 
vpFeatureLine &vpFeatureLine::buildFrom(const double &rho, const double &theta)
{
  s[0] = rho;
  s[1] = theta;
  for (int i = 0; i < 2; i++) {
    flags[i] = true;
  }
  return *this;
}
 
vpFeatureLine &vpFeatureLine::buildFrom(const double &rho, const double &theta, const double &A_, const double &B_, const double &C_, const double &D_)
{
  s[0] = rho;
  s[1] = theta;
  this->A = A_;
  this->B = B_;
  this->C = C_;
  this->D = D_;
  for (unsigned int i = 0; i < nbParameters; ++i) {
    flags[i] = true;
  }
  return *this;
}
 
vpFeatureLine *vpFeatureLine::duplicate() const
{
  vpFeatureLine *feature = new vpFeatureLine;
  return feature;
}
 
void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
                            unsigned int thickness) const
{
  try {
    double rho, theta;
    rho = getRho();
    theta = getTheta();
 
    vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
 
  }
  catch (...) {
    vpERROR_TRACE("Error caught");
    throw;
  }
}
 
void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
                            unsigned int thickness) const
{
  try {
    double rho, theta;
    rho = getRho();
    theta = getTheta();
 
    vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
 
  }
  catch (...) {
    vpERROR_TRACE("Error caught");
    throw;
  }
}
 
unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
 
unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
END_VISP_NAMESPACE