/*!

  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 ;
}
Exemplo n.º 3
0
/*!

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