vpFeaturePoint3D.cpp

/****************************************************************************
 *
 * $Id: vpFeaturePoint3D.cpp 4632 2014-02-03 17:06:40Z 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:
 * 3D point visual feature.
 *
 * Authors:
 * Eric Marchand
 * Fabien Spindler
 *
 *****************************************************************************/


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

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

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





/*

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

*/

void
vpFeaturePoint3D::init()
{
    //feature dimension
    dim_s = 3 ;
    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 XYZ
    s[0] = 0;
    s[1] = 0;
    s[2] = 1;

}

vpFeaturePoint3D::vpFeaturePoint3D()
{
    init() ;
}


void
vpFeaturePoint3D::set_X(const double X)
{
    s[0] = X ;
    flags[0] = true;
}

void
vpFeaturePoint3D::set_Y(const double Y)
{
    s[1] = Y ;
    flags[1] = true;
}

void
vpFeaturePoint3D::set_Z(const double Z)
{
    s[2] = Z ;
    flags[2] = true;
}

void
vpFeaturePoint3D::set_XYZ(const double X,
              const double Y,
              const double Z)
{
  set_X(X) ;
  set_Y(Y) ;
  set_Z(Z) ;

  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
}

double
vpFeaturePoint3D::get_X() const
{
    return s[0] ;
}

double
vpFeaturePoint3D::get_Y() const
{
    return s[1] ;
}

double
vpFeaturePoint3D::get_Z() const
{
    return s[2] ;
}

vpMatrix
vpFeaturePoint3D::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 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();
  }

  double X = get_X() ;
  double Y = get_Y() ;
  double Z = get_Z() ;

  if (vpFeaturePoint3D::selectX() & select )
  {
    vpMatrix Lx(1,6) ; Lx = 0;

    Lx[0][0] = -1  ;
    Lx[0][1] = 0 ;
    Lx[0][2] = 0 ;
    Lx[0][3] = 0 ;
    Lx[0][4] = -Z ;
    Lx[0][5] = Y ;

    L = vpMatrix::stackMatrices(L,Lx) ;
  }

  if (vpFeaturePoint3D::selectY() & select )
  {
    vpMatrix Ly(1,6) ; Ly = 0;

    Ly[0][0] = 0 ;
    Ly[0][1] = -1 ;
    Ly[0][2] = 0 ;
    Ly[0][3] = Z ;
    Ly[0][4] = 0 ;
    Ly[0][5] = -X ;

    L = vpMatrix::stackMatrices(L,Ly) ;
  }
  if (vpFeaturePoint3D::selectZ() & select )
  {
    vpMatrix Lz(1,6) ; Lz = 0;

    Lz[0][0] = 0 ;
    Lz[0][1] = 0 ;
    Lz[0][2] = -1 ;
    Lz[0][3] = -Y ;
    Lz[0][4] = X ;
    Lz[0][5] = 0 ;

    L = vpMatrix::stackMatrices(L,Lz) ;
  }
  return L ;
}

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

  try{
    if (vpFeaturePoint3D::selectX() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0] - s_star[0] ;

      e = vpMatrix::stackMatrices(e,ex) ;
    }

    if (vpFeaturePoint3D::selectY() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] - s_star[1] ;
      e =  vpMatrix::stackMatrices(e,ey) ;
    }

    if (vpFeaturePoint3D::selectZ() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] - s_star[2] ;
      e =  vpMatrix::stackMatrices(e,ez) ;
    }
  }
  catch(vpMatrixException me)
  {
    vpERROR_TRACE("caught a Matrix 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
vpFeaturePoint3D::buildFrom(const vpPoint &p)
{

  // cP is expressed in homogeneous coordinates
  // we devide by the fourth coordinate
  s[0] = p.cP[0]/p.cP[3]  ;
  s[1] = p.cP[1]/p.cP[3]  ;
  s[2] = p.cP[2]/p.cP[3]  ;

  double Z = s[2] ;
  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
vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
{

  s[0] = X ;
  s[1] = Y ;
  s[2] = 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
vpFeaturePoint3D::print(const unsigned int select ) const
{

  std::cout <<"Point3D:  "  ;
  if (vpFeaturePoint3D::selectX() & select )
    std::cout << " X=" << get_X() ;
  if (vpFeaturePoint3D::selectY() & select )
    std::cout << " Y=" << get_Y() ;
  if (vpFeaturePoint3D::selectZ() & select )
    std::cout << " Z=" << get_Z() ;
  std::cout <<std::endl ;
}


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

void
vpFeaturePoint3D::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
vpFeaturePoint3D::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:
 */