vpFeatureDepth.cpp

/****************************************************************************
 *
 * $Id: vpFeatureDepth.cpp 4649 2014-02-07 14:57:11Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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 point visual feature.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/


#include <visp/vpBasicFeature.h>
#include <visp/vpFeatureDepth.h>

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

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

// math
#include <visp/vpMath.h>

#include <visp/vpFeatureDisplay.h>



/*



attributes and members directly related to the vpBasicFeature needs
other functionalities ar useful but not mandatory





*/

void
vpFeatureDepth::init()
{
    //feature dimension
    dim_s = 1 ;
    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;
    x = y = 0.;
    Z = 1.;
}


vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.)
{
    init() ;
}


void
vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar)
{
    s[0] = LogZoverZstar ;
}


double
vpFeatureDepth::get_LogZoverZstar() const
{
    return s[0] ;
}


void
vpFeatureDepth::set_x(const double x_)
{
    this->x = x_ ;
    flags[0] = true;
}


double
vpFeatureDepth::get_x() const
{
    return x ;
}


void
vpFeatureDepth::set_y(const double y_)
{
    this->y = y_ ;
    flags[1] = true;
}


double
vpFeatureDepth::get_y() const
{
    return y ;
}

void
vpFeatureDepth::set_Z(const double Z_)
{
    this->Z = Z_ ;
    flags[2] = true;
}


double
vpFeatureDepth::get_Z() const
{
    return Z ;
}


void
vpFeatureDepth::set_xyZLogZoverZstar(const double x_,
      const double y_,
      const double Z_,
            const double LogZoverZstar)
{
  set_x(x_) ;
  set_y(y_) ;
  set_Z(Z_) ;
  set_LogZoverZstar(LogZoverZstar) ;
  for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
}


vpMatrix
vpFeatureDepth::interaction(const 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 x was not set yet");
        break;
        case 1:
          vpTRACE("Warning !!!  The interaction matrix is computed but y 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();
  }

  L.resize(1,6) ;

  double x_ = get_x();
  double y_ = get_y();
  double Z_ = get_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")) ;
  }

  if (FEATURE_ALL & select)
  {
    L = 0;
    L[0][0] = 0;
    L[0][1] = 0;
    L[0][2] = -1/Z_;
    L[0][3] = -y_;
    L[0][4] = x_;
    L[0][5] = 0;
  }

  return L ;
}


vpColVector
vpFeatureDepth::error(const vpBasicFeature &s_star,
               const unsigned int select)
{

  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
    {
      vpERROR_TRACE("s* should be zero ! ") ;
      throw(vpFeatureException(vpFeatureException::badInitializationError,
                   "s* should be zero !")) ;
    }

  vpColVector e(1) ;
  if(FEATURE_ALL & select)
  {
    e[0] = s[0];
  }

  return e ;
}


void
vpFeatureDepth::print(const unsigned int select ) const
{
  if (FEATURE_ALL & select)
  {
    std::cout <<"Point:  x=" << get_x() ;
    std::cout <<" Point:  y=" << get_y() ;
    std::cout <<" Point:  Z=" << get_Z() ;

    std::cout << " log(Z/Z*)=" << get_LogZoverZstar() ;

    std::cout <<std::endl ;
  }
}


void
vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
{

  s[0] = LogZoverZstar;

  this->x = x_  ;
  this->y = y_  ;
  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;

}



vpFeatureDepth *vpFeatureDepth::duplicate() const
{
  vpFeatureDepth *feature = new vpFeatureDepth;
  return feature ;
}

void
vpFeatureDepth::display(const vpCameraParameters &/* cam */,
                        const vpImage<unsigned char> &/* I */,
                        const vpColor &/* color */,
                        unsigned int /* thickness */) const
{
  static int firsttime =0 ;

  if (firsttime==0)
  {
    firsttime=1 ;
    vpERROR_TRACE("not implemented") ;
    // Do not throw and error since it is not subject
    // to produce a failure
  } 
}
void
vpFeatureDepth::display(const vpCameraParameters &/* cam */,
                        const vpImage<vpRGBa> &/* I */,
                        const vpColor &/* color */,
                        unsigned int /* thickness */) const
{
  static int firsttime =0 ;

  if (firsttime==0)
  {
    firsttime=1 ;
    vpERROR_TRACE("not implemented") ;
    // Do not throw and error since it is not subject
    // to produce a failure
  }
}

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