vpFeatureLine.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 line visual feature.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#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
*/

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 == NULL)
    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;
}

void vpFeatureLine::buildFrom(double rho, double theta)
{
  s[0] = rho;
  s[1] = theta;
  for (int i = 0; i < 2; i++)
    flags[i] = true;
}

void vpFeatureLine::buildFrom(double rho, double theta, double A_, double B_, double C_, 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;
}

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]; }