vpFeatureLine.cpp

/****************************************************************************
 *
 * $Id: vpFeatureLine.cpp 4056 2013-01-05 13:04:42Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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 <visp/vpBasicFeature.h>
#include <visp/vpFeatureLine.h>

// Exception
#include <visp/vpException.h>
#include <visp/vpMatrixException.h>
#include <visp/vpFeatureException.h>

// Debug trace
#include <visp/vpDebug.h>

// simple math function (round)
#include <visp/vpMath.h>

// Display Issue

// Meter/pixel conversion
#include <visp/vpCameraParameters.h>

//Color / image / display
#include <visp/vpColor.h>
#include <visp/vpImage.h>



#include <visp/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() : vpBasicFeature()
{
    init() ;
}


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


void
vpFeatureLine::setABCD(const double A, const double B,
               const double C, const 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(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 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 = vpMatrix::stackMatrices(L,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 = vpMatrix::stackMatrices(L,Ltheta) ;
  }
  return L ;
}


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

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



      e = vpMatrix::stackMatrices(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 =  vpMatrix::stackMatrices(e,etheta) ;
    }
  }
  catch(vpMatrixException me)
  {
    vpERROR_TRACE("caught a Matric related error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }


  return e ;

}


void
vpFeatureLine::print(const 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(const double rho, const double theta)
{
  s[0] = rho ;
  s[1] = theta ;
  for( int i = 0; i < 2; i++) flags[i] = true;
}


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


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



/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */