/*!
\brief create a new visual features in the task
(the desired feature is then null)
*/
void
vpServo::addFeature(vpBasicFeature& s, int select)
{
featureList += &s ;
// in fact we have a problem with s_star that is not defined
// by the end user.
// If the same user want to compute the interaction at the desired position
// this "virtual feature" must exist
// a solution is then to duplicate the current feature (s* = s)
// and reinitialized s* to 0
// it remains the deallocation issue therefore a flag that stipulates that
// the feature has been allocated in vpServo is set
// vpServo must deallocate the memory (see ~vpServo and kill() )
vpBasicFeature *s_star ;
s_star = s.duplicate() ;
s_star->init() ;
s_star->setDeallocate(vpBasicFeature::vpServo) ;
desiredFeatureList += s_star ;
featureSelectionList+= select ;
}
/*!
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 ;
}
/*!
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 ;
}
//! Compute the error between two visual features from a subset of the possible features.
vpColVector vpBasicFeature::error(const vpBasicFeature &s_star,
const unsigned int select)
{
vpColVector e(0),eLine(1);
if (dim_s <= 31)
{
for(unsigned int i=0;i<dim_s;++i){
if(FEATURE_LINE[i] & select)
{
eLine[0] = s[i] - s_star[i];
e.stackMatrices(eLine);
//std::cout << "dim_s <= 31"<<std::endl;
}
}
}
else
{
e.resize(dim_s);
vpColVector sd = s_star.get_s();
e = s - sd;
}
return e ;
}
/*!
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 ;
}
