C++ vpMatrix::resize方法代码示例

/*!

  Compute and return the interaction matrix \f$ L_I \f$. The computation is made
  thanks to the values of the luminance features \f$ I \f$
*/
void
vpFeatureLuminance::interaction(vpMatrix &L)
{
  double x,y,Ix,Iy,Zinv;

  L.resize(dim_s,6) ;

  for(int m = 0; m< L.getRows(); m++)
    {
      Ix = pixInfo[m].Ix;
      Iy = pixInfo[m].Iy;

      x = pixInfo[m].x ;
      y = pixInfo[m].y ;
      Zinv =  1 / pixInfo[m].Z;

      {
	L[m][0] = Ix * Zinv;
	L[m][1] = Iy * Zinv;
	L[m][2] = -(x*Ix+y*Iy)*Zinv;
	L[m][3] = -Ix*x*y-(1+y*y)*Iy;
	L[m][4] = (1+x*x)*Ix + Iy*x*y;
	L[m][5]  = Iy*x-Ix*y;
      }
    }
}

void
vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe)
{

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  fJe.resize(6,2) ;

  double s1 = sin(q[0]) ;
  double c1 = cos(q[0]) ;

  fJe = 0;

  if (dh_model_ == DH1)
  {
    fJe[3][1] = -s1;
    fJe[4][1] = c1;
    fJe[5][0] = 1;
  }
  else
  {
    fJe[3][1] = s1;
    fJe[4][1] = -c1;
    fJe[5][0] = 1;
  }
}

/*!
  Get the robot jacobian expressed in the end-effector frame.

  \warning Re is not the embedded camera frame. It corresponds to the frame
  associated to the tilt axis (see also get_cMe).

  \param q : Articular position for pan and tilt axis.

  \param eJe : Jacobian between end effector frame and end effector frame (on
  tilt axis).

*/
void
vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe)
{


  eJe.resize(6,2) ;

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  double s2 = sin(q[1]) ;
  double c2 = cos(q[1]) ;

  eJe = 0;

  if (dh_model_ == DH1)
  {
    eJe[3][0] = -c2;
    eJe[4][1] = 1;
    eJe[5][0] = -s2;
  }
  else
  {
    eJe[3][0] = -c2;
    eJe[4][1] = -1;
    eJe[5][0] = s2;

  }

}

/*!

  Get the inverse jacobian.

  \f[
  {^f}J_e^+ = \left[\begin{array}{cccccc}
  -(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\
  0 & 0 & 1 & 0 & 0 & 0  \\
  (a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\
  0 & 0 & 0 & c_{14} & s_{14} & 0  \\
  \end{array}
  \right]
  \f]

  \param q : Articular position of the four joints: q[0] corresponds to
  the first rotation (joint 1 with value \f$q_1\f$) of the turret
  around the vertical axis, while q[1] corresponds to the vertical
  translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
  correspond to the pan and tilt of the camera (respectively joint 4
  and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
  q[3] are expressed in radians. The translation q[1] is expressed in
  meters.

  \param fJe_inverse : Inverse robot jacobian expressed in the robot
  reference frame.

  \sa get_eJe() and get_fJe()

*/
void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
{
    fJe_inverse.resize(4, 6) ;
    fJe_inverse = 0;

    double q1 = q[0]; // rot touret
    double q4 = q[2]; // pan

    double c1 = cos(q1);
    double s1 = sin(q1);
    double c14 = cos(q1 + q4);
    double s14 = sin(q1 + q4);

    double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;

    fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
    fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;

    fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;

    fJe_inverse[2][0] = - fJe_inverse[0][0];
    fJe_inverse[2][1] = - fJe_inverse[0][1];

    fJe_inverse[3][3] = c14;
    fJe_inverse[3][4] = s14;
}

void
vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
    fJe.resize(6,4) ;

    double q1 = q[0]; // rot touret
    double q4 = q[2]; // pan

    double c1 = cos(q1);
    double s1 = sin(q1);
    double c14 = cos(q1 + q4);
    double s14 = sin(q1 + q4);

    fJe = 0;

    fJe[0][0] = -s1*this->_a1 - c1*this->_d3;

    fJe[1][0] = c1*this->_a1 - s1*this->_d3;

    fJe[2][1] = 1.0;

    fJe[3][3] = c14;

    fJe[4][3] = s14;

    fJe[5][0] = fJe[5][2] = 1.0;
}

/*!
  \relates vpMatrix

  Compute the skew symmetric matrix \f$M\f$ of translation vector \f$t\f$
  (matrice de pre-produit vectoriel).

  \f[ \mbox{if} \quad  {\bf t} =  \left( \begin{array}{c} t_x \\ t_y \\ t_z
  \end{array}\right), \quad \mbox{then} \qquad
  M = \left( \begin{array}{ccc}
  0 & -t_z & t_y \\
  t_z & 0 & -t_x \\
  -t_y & t_x & 0
  \end{array}\right)
  \f]

  \param t : Translation vector in input used to compute the skew symmetric
  matrix M.

  \param M : Skew symmetric matrix of translation vector \f$t\f$.
*/
void
vpTranslationVector::skew(const vpTranslationVector &t,vpMatrix &M)
{
    M.resize(3,3) ;
    M[0][0] = 0 ;     M[0][1] = -t[2] ;  M[0][2] = t[1] ;
    M[1][0] = t[2] ;  M[1][1] = 0 ;      M[1][2] = -t[0] ;
    M[2][0] = -t[1] ; M[2][1] = t[0] ;   M[2][2] = 0 ;
}

/*!
  Reshape the column vector in a matrix.
  \param M : the reshaped matrix.
  \param nrows : number of rows of the matrix.
  \param ncols : number of columns of the matrix.
*/
void vpColVector::reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols){
  if(dsize!=nrows*ncols) {
    throw(vpException(vpException::dimensionError,
                      "Cannot reshape (%dx1) column vector in (%dx%d) matrix",
                      rowNum, M.getRows(), M.getCols())) ;
  }
  try {
    if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
  }
  catch(...) {
    throw ;
  }

  for(unsigned int j =0; j< ncols; j++)
    for(unsigned int i =0; i< nrows; i++)
      M[i][j]=data[j*nrows+i];
}

/*!
  Get the robot jacobian expressed in the robot reference frame

  \param q : Articular position for pan and tilt axis.

  \param fJe : Jacobian between reference frame (or fix frame) and end effector
  frame (on tilt axis).

*/
void
vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe)
{

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
  }

  fJe.resize(6,2) ;

  double s1 = sin(q[0]) ;
  double c1 = cos(q[0]) ;

  fJe = 0;

  fJe[3][1] = s1;
  fJe[4][1] = -c1;
  fJe[5][0] = 1;
}

/*!
  Get the robot jacobian expressed in the end-effector frame.

  \warning Re is not the embedded camera frame. It corresponds to the frame
  associated to the tilt axis (see also get_cMe).

  \param q : Articular position for pan and tilt axis.

  \param eJe : Jacobian between end effector frame and end effector frame (on
  tilt axis).

*/
void
vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{


  eJe.resize(6,2) ;

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
  }

  double s2 = sin(q[1]) ;
  double c2 = cos(q[1]) ;

  eJe = 0;

  eJe[3][0] = c2;
  eJe[4][1] = 1;
  eJe[5][0] = s2;
}

/*!
  \brief reshape the colvector in a matrix
  \param m : the reshaped Matrix
  \param nrows : number of rows of the matrix
  \param ncols : number of columns of the matrix
*/
void vpColVector::reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols){
  if(dsize!=nrows*ncols)
  {
    vpERROR_TRACE("\n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t \n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix")) ;
  }
  try 
  {
    if ((m.getRows() != nrows) || (m.getCols() != ncols)) m.resize(nrows,ncols);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  for(unsigned int j =0; j< ncols; j++)
     for(unsigned int i =0; i< nrows; i++)
	  m[i][j]=data[j*ncols+i];
}

static void
computeInteractionMatrixFromList  (/*const*/ vpList<vpBasicFeature *> & featureList,
				   /*const*/ vpList<int> & featureSelectionList,
				   vpMatrix & L)
{
  if (featureList.empty())
    {
      vpERROR_TRACE("feature list empty, cannot compute Ls") ;
      throw(vpServoException(vpServoException::noFeatureError,
			     "feature list empty, cannot compute Ls")) ;
    }

  /* The matrix dimension is not known before the affectation loop.
   * It thus should be allocated on the flight, in the loop.
   * The first assumption is that the size has not changed. A double
   * reallocation (realloc(dim*2)) is done if necessary. In particulary,
   * [log_2(dim)+1] reallocations are done for the first matrix computation.
   * If the allocated size is too large, a correction is done after the loop.
   * The algotithmic cost is linear in affectation, logarthmic in allocation
   * numbers and linear in allocation size.
   */

  /* First assumption: matrix dimensions have not changed. If 0, they are
   * initialized to dim 1.*/
  int rowL = L .getRows();
  const int colL = 6;
  if (0 == rowL) { rowL = 1; L .resize(rowL, colL);}

  /* vectTmp is used to store the return values of functions get_s() and
   * error(). */
  vpMatrix matrixTmp;
  int rowMatrixTmp, colMatrixTmp;

  /* The cursor are the number of the next case of the vector array to
   * be affected. A memory reallocation should be done when cursor
   * is out of the vector-array range.*/
  int cursorL = 0;

  for (featureList.front() ,featureSelectionList.front() ;
       !featureList.outside();
       featureSelectionList.next(),featureList.next() )
    {
      vpBasicFeature * sPTR = featureList.value() ;
      const int select = featureSelectionList.value() ;

      /* Get s. */
      matrixTmp = sPTR->interaction(select);
      rowMatrixTmp = matrixTmp .getRows();
      colMatrixTmp = matrixTmp .getCols();

      /* Check the matrix L size, and realloc if needed. */
      while (rowMatrixTmp + cursorL > rowL)
	{ rowL *= 2; L .resize (rowL,colL,false); vpDEBUG_TRACE(15,"Realloc!"); }

      /* Copy the temporarily matrix into L. */
      for (int k = 0; k < rowMatrixTmp; ++k, ++cursorL)
	for (int j = 0; j <  colMatrixTmp; ++j)
	  { L[cursorL][j] = matrixTmp[k][j]; }

    }

  L.resize (cursorL,colL,false);

  return ;
}