/*!
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;
}
/*!
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;
}
/*!
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 ;
}
/*!
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 ;
}
/*!
\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 ;
}
}
/*!
\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];
}
/*!
\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 ;
}
/*!
\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_ ;
}
/*!
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 ;
}
/*!
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 \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 ;
}
/*!
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 ;
}
