C++ vpMatrix类代码示例

/*!
  \brief Initialisation of a sub matrix
  \param m : parent matrix
  \param row : row offset 
  \param col : col offset 
  \param nrows : number of rows of the sub matrix
  \param ncols : number of columns of the sub matrix
*/
void vpSubMatrix::init(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows ,  const unsigned int & ncols){
  
  if(! m.data){
    vpERROR_TRACE("\n\t\t SubMatrix parent matrix is not allocated") ;
    throw(vpMatrixException(vpMatrixException::subMatrixError,
			    "\n\t\t SubMatrix parent matrix is not allocated")) ;
  } 
  
  if(row+nrows <= m.getRows() && col+ncols <= m.getCols()){	
    data=m.data;
    parent =&m; 
    rowNum = nrows;
    colNum = ncols;
    pRowNum=m.getRows(); 
    pColNum=m.getCols(); 
    
    if(rowPtrs)
      free(rowPtrs);
    
    rowPtrs=(double**) malloc(nrows * sizeof(double*));
    for(unsigned int r=0;r<nrows;r++)
      rowPtrs[r]= m.data+col+(r+row)*pColNum;
    
    dsize = pRowNum*pColNum ;
    trsize =0 ;
  }else{
    vpERROR_TRACE("Submatrix cannot be contain in parent matrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Submatrix cannot be contain in parent matrix")) ;
  }
}

/*!

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

/*!
  initialise the camera from a calibration matrix. 
  Using a calibration matrix leads to a camera without distorsion
  
  The K matrix in parameters must be like:
  
  \f$ K = \left(\begin{array}{ccc}
  p_x & 0 & u_0 \\
  0 & p_y & v_0  \\
  0 & 0 & 1
  \end{array} \right) \f$
  
  \param _K : the 3by3 calibration matrix
*/
void
vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K)
{
  if(_K.getRows() != 3 || _K.getCols() != 3 ){
    throw vpException(vpException::dimensionError, "bad size for calibration matrix");
  }
  if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){
    throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
  }
  initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
}

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

/*!
  Apply a filter to an image.

  \param I : Image to filter
  \param If : Filtered image.
  \param M : Filter coefficients.

*/
void
vpImageFilter::filter(const vpImage<unsigned char> &I,
		      vpImage<double>& If,
		      const vpMatrix& M)
{

  unsigned int size = M.getRows() ;
  unsigned int half_size = size/2 ;

  If.resize(I.getHeight(),I.getWidth()) ;

  If = 0 ;

  for (unsigned int i=half_size ; i < I.getHeight()-half_size ; i++)
  {
    for (unsigned int j=half_size ; j < I.getWidth()-half_size ; j++)
    {
      double   conv_x = 0 ;

      for(unsigned int a = 0 ; a < size ; a++ )
        for(unsigned int b = 0 ; b < size ; b++ )
	{
	  double val =  I[i-half_size+a][j-half_size+b] ;
	  conv_x += M[a][b] * val ;
	}
      If[i][j] = conv_x ;
    }
  }

}

/*!
  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;

  }

}

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

/*!
  Apply a filter to an image.

  \param I : Image to filter
  \param Iu : Filtered image along the horizontal axis (u = columns).
  \param Iv : Filtered image along the vertical axis (v = rows).
  \param M : Separate filter coefficients

*/
void
vpImageFilter::filter(const vpImage<double> &I,
		      vpImage<double>& Iu,
		      vpImage<double>& Iv,
		      const vpMatrix& M)
{

  unsigned int size = M.getRows() ;
  unsigned int half_size = size/2 ;

  Iu.resize(I.getHeight(),I.getWidth()) ;
  Iv.resize(I.getHeight(),I.getWidth()) ;

  Iu = 0 ;
  Iv = 0 ;
  for (unsigned int v=half_size ; v < I.getHeight()-half_size ; v++)
  {
    for (unsigned int u=half_size ; u < I.getWidth()-half_size ; u++)
    {
      double   conv_u = 0 ;
      double   conv_v = 0 ;

      for(unsigned int a = 0 ; a < size ; a++ )
        for(unsigned int b = 0 ; b < size ; b++ )
	{
	  double val =  I[v-half_size+a][u-half_size+b] ;
	  conv_u += M[a][b] * val ;
	  conv_v += M[b][a] * val  ;
	}
      Iu[v][u] = conv_u ;
      Iv[v][u] = conv_v ;
    }
  }

}

void KalmanFilter<DataTypes, DepthTypes>::predict(vpMatrix& stiffnessMatrix)
{
	
	predictedForces = estimatedForces;
	predictedPositions = estimatedPositions + stiffnessMatrix.pseudoInverse()*estimatedForces;
	
	
	for (int k = 0; k < 2*predictedForces.getCols(); k++)
	for (int l = 0; l < 2*predictedForces.getCols(); l++)
	std::cout <<k<<" "<<l<<"   "<< stiffnessMatrix[k][l] << std::endl;
	
	estimatedState = estimatedPositions;
	estimatedState.stack(estimatedForces);
	
	predictedState = predictedPositions;
	predictedState.stack(predictedForces);
	
	for (int k = predictedForces.getCols(); k < 2*predictedForces.getCols(); k++)
	for (int l = predictedForces.getCols(); l < 2*predictedForces.getCols(); l++)
		J[k][l] = stiffnessMatrix[k-predictedForces.getCols()][l-predictedForces.getCols()];
		
	PPred = J*PEst*J.transpose() + Q;
    //PfPred = PsEst + Qf;

}

/*!

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

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

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

vpColVector computeSecondaryTaskManipulability(const vpMatrix &P, vpMatrix &J, std::vector <vpMatrix> &dJ,double & cond)
{
    const unsigned int n = dJ.size();

    vpColVector z(n);

    double alpha = 10;

//    vpMatrix v;
//    vpColVector w;
//    J.svd(w, v);
//    //std::cout << "singular values:/n" << w << std::endl;
//    cond = w[0]/w[5];

//    double sqtr_detJJt = 1.0;

//    for (unsigned int i = 0; i < n-1; i++)
//        sqtr_detJJt *= w[i];
//    std::cout << "sqtr_detJJt:/n" << sqtr_detJJt << std::endl;


//    std::cout << "detJJt" << detJJt << std::endl;
    double detJJt = (J * J.transpose()).det();
     std::cout << "sqtr_detJJMulti" << sqrt(detJJt) << std::endl;

    for (unsigned int i = 0; i < n; i++)
    {
        vpMatrix dJJinv = dJ[i] * J.pseudoInverse();

        std::cout << dJ[i]  << std::endl << dJ[i] << std::endl;

        double trace = 0.0;
        for (unsigned int k = 0; k < dJJinv.getCols(); k++)
            trace += dJJinv[k][k];

        std::cout << "trace" << i << " " << trace << std::endl;

        z[i] = alpha * sqrt(detJJt) * trace;


    }

    return z;

}

// writes a matrix into a single line YAML format
string okExperiment::singleLineYAML(const vpMatrix &M)
{
    stringstream ss;
    unsigned int i,j;

    ss << "[";
    for(i=0;i<M.getRows();++i)
    {
        ss << "[";
        for(j=0;j<M.getCols()-1;++j)
            ss << M[i][j] << ", ";
        ss << M[i][j] << "]";
        if(i != M.getRows()-1)
            ss << ",";
    }
    ss << "]";
    return ss.str();
}