vpFeaturePointPolar.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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] ; }