/*!  
  
  Build a 3D point visual feature from the camera frame coordinates
  \f$(X,Y,Z)\f$ of a point.

  \param X,Y,Z : Camera frame coordinates \f$(X,Y,Z)\f$ of a 3D point.

  \exception vpFeatureException::badInitializationError: If the depth
  (\f$Z\f$ coordinate) is negative. That means that the 3D point is
  on the camera which is not possible.

  \exception vpFeatureException::badInitializationError: If the depth
  (\f$Z\f$ coordinate) is null. That means that the 3D point is
  on the camera which is not possible.

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

}
/*!  
  
  Build a 3D point visual feature from the camera frame coordinates
  \f$(X,Y,Z)\f$ of a point.

  \param p : A point with camera frame coordinates \f${^c}P=(X,Y,Z)\f$
  up to date (see vpPoint class).

  \exception vpFeatureException::badInitializationError: If the depth
  (\f$Z\f$ coordinate) is negative. That means that the 3D point is
  behind the camera which is not possible.

  \exception vpFeatureException::badInitializationError: If the depth
  (\f$Z\f$ coordinate) is null. That means that the 3D point is
  on the camera which is not possible.
*/
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;

}
Esempio n. 3
0
/*!

  Build a 2D image point visual feature with polar coordinates.

  \param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
  the image point.

  \param Z : 3D depth of the point in the camera frame.

  \exception vpFeatureException::badInitializationError: If the depth
  (\f$Z\f$ coordinate) is negative. That means that the 3D point is
  behind the camera which is not possible.

  \exception vpFeatureException::badInitializationError: If the depth
  (\f$Z\f$ coordinate) is null. That means that the 3D point is
  on the camera which is not possible.
*/
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( int i = 0; i < nbParameters; i++) flags[i] = true;
}
Esempio n. 4
0
/*!

  Compute and return the interaction matrix \f$ L \f$ for the whole
  features or a part of them.

  \param select : Selection of a subset of the possible features. 
  - To compute the interaction matrix for all the features use
    vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
    matrix is \f$ [number of features \times 6] \f$
  - To compute the interaction matrix for only one of the component
    feature you have to say which one you want to take into
    account. If it is the first one set select to
    vpBasicFeature::FEATURE_LINE[0], if it is the second one set
    select to vpBasicFeature::FEATURE_LINE[1], and so on. In that case
    the returned interaction matrix is \f$ [1 \times 6] \f$ dimension.
  - To compute the interaction matrix for only two of the component
    features you have to say which ones you want to take into
    account. If it is the first one and the second one set select to
    vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
    that case the returned interaction matrix is \f$ [2 \times 6] \f$
    dimension.

  \return The interaction matrix computed from the features.

  The code below shows how to compute the interaction matrix associated to the
  first visual feature.
  \code
  // Creation of the current feature s
  vpGenericFeature s(3);
  s.set_s(0, 0, 0);

  // Here you have to compute the interaction matrix L for all the three features
  s.setInteractionMatrix(L);

  vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0] );
  \endcode

  The code below shows how to compute the interaction matrix associated to two
  visual features over three.
  \code
  // Creation of the current feature s
  vpGenericFeature s(3);
  s.set_s(0, 0, 0);

  // Here you have to compute the interaction matrix L
  s.setInteractionMatrix(L);

  vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0]|vpBasicFeature::FEATURE_LINE[1] );
  \endcode
*/
vpMatrix
vpGenericFeature::interaction(const unsigned int select)
{
  if (L.getRows() == 0)
  {
    std::cout << "interaction matrix " << L << std::endl ;
    vpERROR_TRACE("Interaction has not been initialized");
    std::cout << "A possible reason (may be) is that you have set" << std::endl ;
    std::cout << "the interaction matrix for s and compute a control " << std::endl ;
    std::cout << "with Ls=s* (default) or vice versa" << std::endl ;

    throw(vpFeatureException(vpFeatureException::notInitializedError,
			     "size mismatch between s* dimension "
			     "and feature dimension"));

  }

  vpMatrix Ls ;

  Ls.resize(0,6) ;

  for (unsigned int i=0 ; i < dim_s ; i++)
    if (FEATURE_LINE[i] & select )
    {
      vpMatrix Lx(1,6) ; Lx = 0;

      for (int j=0 ; j < 6 ; j++)
	Lx[0][j] = L[i][j] ;

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

  return Ls ;
}
Esempio n. 5
0
/*!

  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features. But in this
  case the desired feature is considered as set to 0.

  \param select : The error can be computed for a selection of a
  subset of the possible features.
  - To compute the error for all the features use
    vpBasicFeature::FEATURE_ALL. In that case the error vector column
    vector whose dimension is equal to the number of features.
  - To compute the error for only one of the component feature you
    have to say which one you want to take into account. If it is the
    first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
    the second one set select to vpBasicFeature::FEATURE_LINE[1], and
    so on. In that case the error vector is a 1 dimension column
    vector.
  - To compute the error for only two of the component feature you
    have to say which ones you want to take into account. If it is the
    first one and the second one set select to
    vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
    that case the error vector is a 2 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature which is automatically set to zero.

  The code below shows how to use this method to manipulate the two
  visual features over three:
  \code
  // Creation of the current feature s
  vpGenericFeature s(3);
  s.set_s(0, 0, 0);

  // Here you have to compute the interaction matrix L
  s.setInteractionMatrix(L);

  // Compute the error vector (s-s*) for the two first features
  s.error(vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
  \endcode
*/
vpColVector
vpGenericFeature::error( const unsigned int select)
{
  vpColVector e(0) ;

  try
  {
    if (errorStatus == errorHasToBeUpdated)
    {
      vpERROR_TRACE("Error has no been updated since last iteration"
		  "you should have used vpGenericFeature::setError"
		  "in you visual servoing loop") ;
      throw(vpFeatureException(vpFeatureException::badErrorVectorError,
			       "Error has no been updated since last iteration"));
    }
    else
      if (errorStatus == errorInitialized)
      {
	errorStatus = errorHasToBeUpdated ;
	for (unsigned int i=0 ; i < dim_s ; i++)
	  if (FEATURE_LINE[i] & select )
	  {
	    vpColVector ex(1) ;
	    ex[i] = err[i] ;

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

	for (unsigned int i=0 ; i < dim_s ; i++)
	  if (FEATURE_LINE[i] & select )
	  {
	    vpColVector ex(1) ;
	    ex[i] = s[i]  ;

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

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

}
Esempio n. 6
0
/*!
  \brief get the value of all the features.

  \param s_vector : It is a vector which will contain the value of the visual features.

  \exception an exception is thrown if the number of row of the vector s
  is different from the dimension of the visual feature as specified
  in the constructor
*/
void
vpGenericFeature::get_s(vpColVector &s_vector) const
{
  if (s_vector.getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between s dimension"
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between s dimension"
			     "and feature dimension"));
  }
  s_vector = this->s ;
}
/*!

  Initialize a point feature with polar coordinates
  \f$(\rho,\theta)\f$ using the coordinates of the point
  \f$(x,y,Z)\f$, where \f$(x,y)\f$ correspond to the perspective
  projection of the point in the image plane and \f$Z\f$ the 3D depth
  of the point in the camera frame. The values of \f$(x,y,Z)\f$ are
  expressed in meters. From the coordinates in the image plane, the
  polar coordinates are computed by:

  \f[\rho = \sqrt{x^2+y^2}  \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]

  \param s : Visual feature \f$(\rho,\theta)\f$ and \f$Z\f$ to initialize.

  \param p : A point with \f$(x,y)\f$ cartesian coordinates in the
  image plane corresponding to the camera perspective projection, and
  with 3D depth \f$Z\f$.
*/
void
vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p)
{
  try {

    double x = p.get_x();
    double y = p.get_y();

    double rho   = sqrt(x*x + y*y);
    double theta = atan2(y, x);

    s.set_rho(rho) ;
    s.set_theta(theta) ;

    s.set_Z( p.get_Z() )  ;


    if (s.get_Z() < 0) {
      vpERROR_TRACE("Point is behind the camera ") ;
      std::cout <<"Z = " << s.get_Z() << std::endl ;

      throw(vpFeatureException(vpFeatureException::badInitializationError,
			       "Point is behind the camera ")) ;
    }

    if (fabs(s.get_Z()) < 1e-6) {
      vpERROR_TRACE("Point Z coordinates is null ") ;
      std::cout <<"Z = " << s.get_Z() << std::endl ;

      throw(vpFeatureException(vpFeatureException::badInitializationError,
			       "Point Z coordinates is null")) ;
    }

  }
  catch(...) {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
}
Esempio n. 8
0
/*!
  \brief get the value of one feature if the number of feature is equal to 1.

  \param s0 : value of the visual feature

  \exception an exception is thrown if the number of row of the vector s
  is different from the dimension of the visual feature as specified
  in the constructor
*/
void
vpGenericFeature::get_s(double &s0) const
{

  if (1 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between  number of parameters"
			     "and feature dimension"));

  }
  s0 = s[0];
}
Esempio n. 9
0
/*!
  \brief set the value of two features if the number of feature is equal to 2.

  \param s0 : value of the first visual feature

  \param s1 : value of the second visual feature

  \exception an exception is thrown if the number of row of the vector s
  is different from the dimension of the visual feature as specified
  in the constructor
*/
void
vpGenericFeature::set_s(const double s0, const double s1)
{

  if (2 != dim_s)
  {
    vpERROR_TRACE("size mismatch between number of parameters"
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between  number of parameters"
			     "and feature dimension"));

  }
  s[0] = s0 ; s[1] = s1 ;
}
Esempio n. 10
0
/*!
  \brief set the value of the interaction matrix.

  \param L_ : The matrix corresponding to the interaction matrix you computed.

  \exception an exception is thrown if the number of row of the interaction
  matrix is different from the dimension of the visual feature as specified
  in the constructor
*/
void
vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
{
  if (L_.getRows() != dim_s)
  {
    std::cout << L_.getRows() <<"  " << dim_s << std::endl ;;
    vpERROR_TRACE("size mismatch between interaction matrix size "
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between interaction matrix size "
			     "and feature dimension"));
  }

  this->L = L_ ;
}
Esempio n. 11
0
/*!

  Set the error vector \f$(s-s*)\f$.

  \param error_vector : Error vector \f$(s-s*)\f$.

  \exception vpFeatureException::sizeMismatchError : If the size of
  the error vector is bad.
*/
void
vpGenericFeature::setError(const vpColVector &error_vector)
{
  if (error_vector.getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between error dimension"
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between error dimension"
			     "and feature dimension"));

  }
  errorStatus = errorInitialized ;
  err = error_vector ;
}
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  - With the feature type cdMc:
  Since this visual feature \f$ s \f$ represent the 3D translation from the desired
  camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
  visual feature \f$ s^* \f$ should be zero. Thus, the error is here
  equal to the current visual feature \f$ s \f$.

  - With the feature type cMo:
  In this case the desired feature is not necessary equal to zero. Thus, the error is here equal to \f$ s-s^* \f$.

  \param s_star : Desired visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible translation features.
  - To compute the error for all the three translation vector coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the translation vector coordinate
    feature \f$(t_x, t_y, t_z)\f$ use one of the
    corresponding function selectTx(), selectTy() or selectTz(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  \exception vpFeatureException::badInitializationError : If the
  desired visual feature \f$ s^* \f$ is not equal to zero in the case of the feature type is cdMc or cMcd.

  The code below shows how to use this method to manipulate the \f$
  t_z \f$ subset in the case of the cdMc feature type. It can be used also with the cMo feature type. In that case just change vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the declaration of the two vpFeatureTranslation features.

  \code
  // Creation of the current feature s
  vpFeatureTranslation s(vpFeatureTranslation::cdMc);
  s.set_TUz(0.3); // Initialization of the feature

  // Creation of the desired feature s*. By default this feature is 
  // initialized to zero
  vpFeatureTranslation s_star(vpFeatureTranslation::cdMc); 

  // Compute the interaction matrix for the t_z translation feature
  vpMatrix L_z = s.interaction( vpFeatureTranslation::selectTz() );

  // Compute the error vector (s-s*) for the t_z feature
  s.error(s_star, vpFeatureTranslation::selectTz());
  \endcode

  To manipulate the subset features \f$s=(t_y, t_z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the t_y, t_z features
  vpMatrix L_yz = s.interaction( vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz() );

  // Compute the error vector e = (s-s*) for the t_y, t_z feature
  vpColVector e = s.error(s_star, vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz());
  \endcode

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

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


  if (vpFeatureTranslation::selectTx() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0]-s_star[0]  ;
      e = vpMatrix::stackMatrices(e,ex) ;
    }

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

  if (vpFeatureTranslation::selectTz() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2]-s_star[2] ;
      e = vpMatrix::stackMatrices(e,ez) ;
    }

  return e ;
}
Esempio n. 13
0
/*!
  
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  Since this visual feature \f$ s \f$ represent either the rotation
  from the desired camera frame to the current camera frame
  \f$^{c^*}R_{c} \f$, or the rotation from the current camera frame to
  the desired camera frame \f$^{c}R_{c^*} \f$, the desired visual
  feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
  the current visual feature \f$ s \f$.

  \param s_star : Desired visual visual feature that should be equal to zero.

  \param select : The error can be computed for a selection of a
  subset of the possible \f$ \theta u \f$ features.
  - To compute the error for all the three \f$ \theta u \f$ features use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the \f$ \theta u \f$ component 
    feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
    corresponding function selectTUx(), selectTUy() or selectTUz(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  \exception vpFeatureException::badInitializationError : If the
  desired visual feature \f$ s^* \f$ is not equal to zero.

  The code below shows how to use this method to manipulate the
  \f$\theta u_z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeatureThetaU s(vpFeatureThetaU::cdRc);
  s.set_TUz(0.3);

  // Creation of the desired feature s^*. By default this feature is 
  // initialized to zero
  vpFeatureThetaU s_star(vpFeatureThetaU::cdRc); 

  // Compute the interaction matrix for the ThetaU_z feature
  vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );

  // Compute the error vector (s-s*) for the ThetaU_z feature
  s.error(s_star, vpFeatureThetaU::selectTUz());
  \endcode

  To manipulate the subset features \f$s=(\theta u_y, \theta u_z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the ThetaU_y, ThetaU_z features
  vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz() );

  // Compute the error vector e = (s-s*) for the ThetaU_y, ThetaU_z feature
  vpColVector e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz());
  \endcode

*/
vpColVector
vpFeatureThetaU::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(0) ;


  if (vpFeatureThetaU::selectTUx() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0]  ;
      e = vpColVector::stack(e,ex) ;
    }

  if (vpFeatureThetaU::selectTUy() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] ;
      e = vpColVector::stack(e,ey) ;
    }

  if (vpFeatureThetaU::selectTUz() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] ;
      e = vpColVector::stack(e,ez) ;
    }
  return e ;
}
Esempio n. 14
0
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  \exception if errorHasBeenInitialized is true (that is if
  vpGenericFeature::setError have been used) then s_star is useless.  In that
  since the error HAS TO BE recomputed at each iteration
  errorHasBeenInitialized is set to errHasToBeUpdated if
  vpGenericFeature::serError is not used in the loop then an exception is
  thrown

  obviously if vpGenericFeature::setError is not used then s_star is considered
  and this warning is meaningless.

  \param s_star : Desired visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible features.
  - To compute the error for all the features use
    vpBasicFeature::FEATURE_ALL. In that case the error vector column
    vector whose dimension is equal to the number of features.
  - To compute the error for only one of the component feature you
    have to say which one you want to take into account. If it is the
    first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
    the second one set select to vpBasicFeature::FEATURE_LINE[1], and
    so on. In that case the error vector is a 1 dimension column
    vector.
  - To compute the error for only two of the component feature you
    have to say which ones you want to take into account. If it is the
    first one and the second one set select to
    vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
    that case the error vector is a 2 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  The code below shows how to use this method to manipulate the two
  visual features over three:

  \code
  // Creation of the current feature s
  vpGenericFeature s(3);
  s.set_s(0, 0, 0);

  // Creation of the desired feature s*
  vpGenericFeature s_star(3);
  s_star.set_s(1, 1, 1);

  // Here you have to compute the interaction matrix L
  s.setInteractionMatrix(L);

  // Compute the error vector (s-s*) for the two first features
  s.error(s_star, vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
  \endcode
*/
vpColVector
vpGenericFeature::error(const vpBasicFeature &s_star,
			const unsigned int select)
{
  if (s_star.get_s().getRows() != dim_s)
  {
    vpERROR_TRACE("size mismatch between s* dimension "
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between s* dimension "
			     "and feature dimension"));

  }

  vpColVector e(0) ;

  try
  {
    if (errorStatus == errorHasToBeUpdated)
    {
      vpERROR_TRACE("Error has no been updated since last iteration"
		  "you should have used vpGenericFeature::setError"
		  "in you visual servoing loop") ;
      throw(vpFeatureException(vpFeatureException::badErrorVectorError,
			       "Error has no been updated since last iteration"));
    }
    else
      if (errorStatus == errorInitialized)
      {
	vpDEBUG_TRACE(25,"Error init: e=e.");
	errorStatus = errorHasToBeUpdated ;
	for (unsigned int i=0 ; i < dim_s ; i++)
	  if (FEATURE_LINE[i] & select )
	  {
	    vpColVector ex(1) ;
	    ex[i] = err[i] ;

	    e = vpMatrix::stackMatrices(e,ex) ;
	  }
      }
      else
      {
	vpDEBUG_TRACE(25,"Error not init: e=s-s*.");

	for (unsigned int i=0 ; i < dim_s ; i++)
	  if (FEATURE_LINE[i] & select )
	  {
	    vpColVector ex(1) ;
	    ex[0] = s[i] - s_star[i] ;

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

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

}
Esempio n. 15
0
/*!
  Compute and return the interaction matrix \f$ L \f$ associated to a
  subset of the possible 2D image point features with polar
  coordinates \f$(\rho,\theta)\f$.

  \f[
  L = \left[
  \begin{array}{l}
  L_{\rho} \\
  \; \\
  L_{\theta}\\
  \end{array}
  \right]
  =
  \left[
  \begin{array}{cccccc}
  \frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z}  &  \frac{\rho}{Z} & (1+\rho^2)\sin\theta  & -(1+\rho^2)\cos\theta &  0 \\
  \; \\
   \frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} &  0 &  \cos\theta /\rho &  \sin\theta/\rho & -1 \\
  \end{array}
  \right]
  \f]

  where \f$Z\f$ is the 3D depth of the considered point.

  \param select : Selection of a subset of the possible polar
  point coordinate features.
  - To compute the interaction matrix for all the two 
    subset features \f$(\rho,\theta)\f$ use vpBasicFeature::FEATURE_ALL. In
    that case the dimension of the interaction matrix is \f$ [2 \times
    6] \f$
  - To compute the interaction matrix for only one of the subset
    (\f$\rho,\theta\f$) use one of the corresponding function
    selectRho() or selectTheta(). In that case the returned
    interaction matrix is \f$ [1 \times 6] \f$ dimension.

  \return The interaction matrix computed from the 2D point
  polar coordinate features.

  \exception vpFeatureException::badInitializationError : If the point
  is behind the camera \f$(Z < 0)\f$, or if the 3D depth is null \f$(Z
  = 0)\f$, or if the \f$\rho\f$ polar coordinate of the point is null.

  The code below shows how to compute the interaction matrix associated to 
  the visual feature \f$s = (\rho,\theta)\f$.
  \code
  vpFeaturePointPolar s;
  double rho   = 0.3;
  double theta = M_PI;
  double Z     = 1;
  // Creation of the current feature s
  s.buildFrom(rho, theta, Z);
  // Build the interaction matrix L_s
  vpMatrix L = s.interaction();
  \endcode

  The interaction matrix could also be build by:
  \code
  vpMatrix L = s.interaction( vpBasicFeature::FEATURE_ALL );
  \endcode

  In both cases, L is a 2 by 6 matrix. The first line corresponds to
  the \f$\rho\f$ visual feature while the second one to the
  \f$\theta\f$ visual feature.

  It is also possible to build the interaction matrix associated to
  one of the possible features. The code below shows how to consider
  only the \f$\theta\f$ component.

  \code
  vpMatrix L_theta = s.interaction( vpFeaturePointPolar::selectTheta() );
  \endcode

  In that case, L_theta is a 1 by 6 matrix.
*/
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::stackMatrices(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::stackMatrices(L,Ltheta) ;
  }
  return L ;
}