/*!
Compute and return the interaction matrix \f$ L \f$ from a subset
\f$(t_x, t_y, t_z)\f$ of the possible translation features that
represent the 3D transformation \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$.
As it exists three different features, the computation of the
interaction matrix is diferent for each one.
- With the feature type cdMc:
\f[ L = [ ^{c^*}R_c \;\; 0_3] \f]
where \f$^{c^*}R_c\f$ is the rotation the camera has to achieve to
move from the desired camera frame to the current camera frame.
- With the feature type cMcd:
\f[ L = [ -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
where \f$^{c}R_{c^*}\f$ is the rotation the camera has to achieve to
move from the current camera frame to the desired camera frame.
- With the feature type cMo:
\f[ L = [ -I_3 \;\; [^{c}t_o]_\times] \f]
where \f$^{c}t_o \f$ is the position of
the object frame relative to the current camera frame.
\param select : Selection of a subset of the possible translation
features.
- To compute the interaction matrix for all the three translation
subset features \f$(t_x,t_y,t_y)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [3 \times
6] \f$
- To compute the interaction matrix for only one of the translation
subset (\f$t_x, t_y, t_z\f$) use
one of the corresponding function selectTx(), selectTy() or
selectTz(). In that case the returned interaction matrix is \f$ [1
\times 6] \f$ dimension.
\return The interaction matrix computed from the translation
features.
The code below shows how to compute the interaction matrix
associated to the visual feature \f$s = t_x \f$ using the cdMc feature type.
\code
vpHomogeneousMatrix cdMc;
...
// Creation of the current feature s
vpFeatureTranslation s(vpFeatureTranslation::cdMc);
s.buildFrom(cdMc);
vpMatrix L_x = s.interaction( vpFeatureTranslation::selectTx() );
\endcode
The code below shows how to compute the interaction matrix
associated to the \f$s = (t_x, t_y) \f$
subset visual feature:
\code
vpMatrix L_xy = s.interaction( vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy() );
\endcode
L_xy is here now a 2 by 6 matrix. The first line corresponds to
the \f$ t_x \f$ visual feature while the second one to the \f$
t_y \f$ visual feature.
It is also possible to build the interaction matrix from all the
translation components by:
\code
vpMatrix L_xyz = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
In that case, L_xyz is a 3 by 6 interaction matrix where the last
line corresponds to the \f$ t_z \f$ visual feature.
*/
vpMatrix
vpFeatureTranslation::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 f2Mf1 was not set yet");
break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
}
}
resetFlags();
}
if (translation == cdMc) {
//This version is a simplification
if (vpFeatureTranslation::selectTx() & select ) {
vpMatrix Lx(1,6) ;
for (int i=0 ; i < 3 ; i++)
Lx[0][i] = f2Mf1[0][i] ;
Lx[0][3] = 0 ; Lx[0][4] = 0 ; Lx[0][5] = 0 ;
L = vpMatrix::stackMatrices(L,Lx) ;
}
if (vpFeatureTranslation::selectTy() & select ) {
vpMatrix Ly(1,6) ;
for (int i=0 ; i < 3 ; i++)
Ly[0][i] = f2Mf1[1][i] ;
Ly[0][3] = 0 ; Ly[0][4] = 0 ; Ly[0][5] = 0 ;
L = vpMatrix::stackMatrices(L,Ly) ;
}
if (vpFeatureTranslation::selectTz() & select ) {
vpMatrix Lz(1,6) ;
for (int i=0 ; i < 3 ; i++)
Lz[0][i] = f2Mf1[2][i] ;
Lz[0][3] = 0 ; Lz[0][4] = 0 ; Lz[0][5] = 0 ;
L = vpMatrix::stackMatrices(L,Lz) ;
}
}
if (translation == cMcd) {
//This version is a simplification
if (vpFeatureTranslation::selectTx() & select ) {
vpMatrix Lx(1,6) ;
Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
L = vpMatrix::stackMatrices(L,Lx) ;
}
if (vpFeatureTranslation::selectTy() & select ) {
vpMatrix Ly(1,6) ;
Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
L = vpMatrix::stackMatrices(L,Ly) ;
}
if (vpFeatureTranslation::selectTz() & select ) {
vpMatrix Lz(1,6) ;
Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
L = vpMatrix::stackMatrices(L,Lz) ;
}
}
if (translation == cMo) {
//This version is a simplification
if (vpFeatureTranslation::selectTx() & select ) {
vpMatrix Lx(1,6) ;
Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
L = vpMatrix::stackMatrices(L,Lx) ;
}
if (vpFeatureTranslation::selectTy() & select ) {
vpMatrix Ly(1,6) ;
Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
L = vpMatrix::stackMatrices(L,Ly) ;
}
if (vpFeatureTranslation::selectTz() & select ) {
vpMatrix Lz(1,6) ;
Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
L = vpMatrix::stackMatrices(L,Lz) ;
}
}
return L ;
}
/*!
Compute and return the interaction matrix \f$ L \f$ associated to a subset
of the possible 3D point features \f$(X,Y,Z)\f$ that
represent the 3D point coordinates expressed in the camera frame.
\f[
L = \left[
\begin{array}{rrrrrr}
-1 & 0 & 0 & 0 & -Z & Y \\
0 & -1 & 0 & Z & 0 & -X \\
0 & 0 & -1 & -Y & X & 0 \\
\end{array}
\right]
\f]
\param select : Selection of a subset of the possible 3D point coordinate
features.
- To compute the interaction matrix for all the three
subset features \f$(X,Y,Z)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [3 \times
6] \f$
- To compute the interaction matrix for only one of the
subset (\f$X, Y,Z\f$) use
one of the corresponding function selectX(), selectY() or
selectZ(). In that case the returned interaction matrix is \f$ [1
\times 6] \f$ dimension.
\return The interaction matrix computed from the 3D point coordinate
features.
The code below shows how to compute the interaction matrix
associated to the visual feature \f$s = X \f$.
\code
vpPoint point;
...
// Creation of the current feature s
vpFeaturePoint3D s;
s.buildFrom(point);
vpMatrix L_X = s.interaction( vpFeaturePoint3D::selectX() );
\endcode
The code below shows how to compute the interaction matrix
associated to the \f$s = (X,Y) \f$
subset visual feature:
\code
vpMatrix L_XY = s.interaction( vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY() );
\endcode
L_XY is here now a 2 by 6 matrix. The first line corresponds to
the \f$ X \f$ visual feature while the second one to the \f$
Y \f$ visual feature.
It is also possible to build the interaction matrix from all the
3D point coordinates by:
\code
vpMatrix L_XYZ = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
In that case, L_XYZ is a 3 by 6 interaction matrix where the last
line corresponds to the \f$ Z \f$ visual feature.
*/
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 ;
}
/*!
Compute and return the interaction matrix \f$ L \f$ from
a subset (\f$ \theta u_x, \theta u_y, \theta u_z\f$) of the possible
\f$ \theta u \f$ features that represent the 3D rotation
\f$^{c^*}R_c\f$ or \f$^{c}R_{c^*}\f$, with
\f[ L = [ 0_3 \; L_{\theta u}] \f]
See the vpFeatureThetaU class description for the equations of
\f$L_{\theta u}\f$.
\param select : Selection of a subset of the possible \f$ \theta u \f$
features.
- To compute the interaction matrix for all the three \f$ \theta u \f$
features use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
interaction matrix is \f$ [3 \times 6] \f$
- To compute the interaction matrix 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 returned interaction matrix is \f$ [1 \times 6] \f$
dimension.
\return The interaction matrix computed from the \f$ \theta u \f$
features that represent either the rotation \f$^{c^*}R_c\f$ or the
rotation \f$^{c}R_{c^*}\f$.
The code below shows how to compute the interaction matrix
associated to the visual feature \f$s = \theta u_x \f$.
\code
vpRotationMatrix cdMc;
// Creation of the current feature s
vpFeatureThetaU s(vpFeatureThetaU::cdRc);
s.buildFrom(cdMc);
vpMatrix L_x = s.interaction( vpFeatureThetaU::selectTUx() );
\endcode
The code below shows how to compute the interaction matrix
associated to the \f$s = (\theta u_x, \theta u_y) \f$
subset visual feature:
\code
vpMatrix L_xy = s.interaction( vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy() );
\endcode
L_xy is here now a 2 by 6 matrix. The first line corresponds to
the \f$ \theta u_x \f$ visual feature while the second one to the \f$
\theta u_y \f$ visual feature.
It is also possible to build the interaction matrix from all the \f$
\theta u \f$ components by:
\code
vpMatrix L_xyz = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
In that case, L_xyz is a 3 by 6 interaction matrix where the last
line corresponds to the \f$ \theta u_z \f$ visual feature.
*/
vpMatrix
vpFeatureThetaU::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 Tu_x was not set yet");
break;
case 1:
vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
break;
case 2:
vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
}
}
resetFlags();
}
// Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
vpColVector u(3) ;
for (unsigned int i=0 ; i < 3 ; i++) {
u[i] = s[i]/2.0 ;
}
vpMatrix Lw(3,3) ;
Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
vpMatrix U2(3,3) ;
U2.eye() ;
double theta = sqrt(s.sumSquare()) ;
if (theta >= 1e-6) {
for (unsigned int i=0 ; i < 3 ; i++)
u[i] = s[i]/theta ;
vpMatrix skew_u ;
skew_u = vpColVector::skew(u) ;
U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
}
if (rotation == cdRc) {
Lw += U2;
}
else {
Lw -= U2;
}
//This version is a simplification
if (vpFeatureThetaU::selectTUx() & select )
{
vpMatrix Lx(1,6) ;
Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
L = vpMatrix::stack(L,Lx) ;
}
if (vpFeatureThetaU::selectTUy() & select )
{
vpMatrix Ly(1,6) ;
Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
L = vpMatrix::stack(L,Ly) ;
}
if (vpFeatureThetaU::selectTUz() & select )
{
vpMatrix Lz(1,6) ;
Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
L = vpMatrix::stack(L,Lz) ;
}
return L ;
}
