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

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

/*!
   Constructor that creates a column vector from a m-by-1 matrix \e M.

   \exception vpException::dimensionError If the matrix is not a m-by-1 matrix.
 */
vpColVector::vpColVector (const vpMatrix &M)
  : vpArray2D<double>(M.getRows(), 1)
{
  if(M.getCols()!=1) {
    throw(vpException(vpException::dimensionError,
                      "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
                      M.getRows(), M.getRows(), M.getCols())) ;
  }

  for(unsigned int i=0; i< M.getRows(); i++)
    (*this)[i] = M[i][0];
}

/*!
   Constructor that creates a row vector from a 1-by-n matrix \e M.

   \exception vpException::dimensionError If the matrix is not a 1-by-n matrix.
 */
vpRowVector::vpRowVector (const vpMatrix &M)
  : vpArray2D<double>(1, M.getCols())
{
  if(M.getRows()!=1) {
    throw(vpException(vpException::dimensionError,
                      "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
                      M.getCols(), M.getRows(), M.getCols())) ;
  }

  for(unsigned int j=0; j< M.getCols(); j++)
    (*this)[j] = M[0][j];
}

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

/*!
  Operator that allows to multiply a velocity twist transformation matrix by a matrix.

  As shown in the example below, this operator can be used to compute the corresponding
  camera velocity skew from the joint velocities knowing the robot jacobian.

  \code
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>

int main()
{
  vpSimulatorCamera robot;

  vpColVector q_vel(6); // Joint velocity on the 6 joints
  // ... q_vel need here to be initialized
  
  vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz 

  vpVelocityTwistMatrix cVe;  // Velocity skew transformation from camera frame to end-effector
  robot.get_cVe(cVe);

  vpMatrix eJe;       // Robot jacobian
  robot.get_eJe(eJe);

  // Compute the velocity in the camera frame
  c_v = cVe * eJe * q_vel;

  return 0;
}
  \endcode

  \exception vpException::dimensionError If M is not a 6 rows dimension matrix.
*/
vpMatrix
vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
{
  if (6 != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }

  vpMatrix p(6, M.getCols()) ;
  for (unsigned int i=0;i<6;i++)
    for (unsigned int j=0;j<M.getCols();j++)
      {
	double s =0 ;
	for (unsigned int k=0;k<6;k++)
	  s += rowPtrs[i][k] * M[k][j];
	p[i][j] = s ;
      }
  return p;
}

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

/*!
  Operator that allows to multiply a skew transformation matrix by a matrix.

  \exception vpMatrixException::incorrectMatrixSizeError If M is not a 6 rows
  dimension matrix.
*/
vpMatrix
vpForceTwistMatrix::operator*(const vpMatrix &M) const
{

  if (6 != M.getRows())
  {
    vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply") ;
    throw(vpMatrixException::incorrectMatrixSizeError) ;
  }

  vpMatrix p(6, M.getCols()) ;
  for (unsigned int i=0;i<6;i++) {
    for (unsigned int j=0;j<M.getCols();j++) {
      double s =0 ;
      for (unsigned int k=0;k<6;k++)
	s += rowPtrs[i][k] * M[k][j];
      p[i][j] = s ;
    }
  }
  return p;
}

/*!

  Multiply a row vector by a matrix.

  \param M : Matrix.

  \warning The number of elements of the row vector must be equal to the number
  of rows of the matrix.

  \exception vpException::dimensionError If the number of elements of the
  row vector is not equal to the number of rows of the matrix.

  \return The resulting row vector.

*/
vpRowVector vpRowVector::operator*(const vpMatrix &M) const
{
  vpRowVector c(M.getCols());

  if (colNum != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (1x%d) row vector by (%dx%d) matrix",
                      colNum, M.getRows(), M.getCols())) ;
  }

  c = 0.0;

  for (unsigned int i=0;i<colNum;i++) {
    double bi = data[i] ; // optimization em 5/12/2006
    for (unsigned int j=0;j<M.getCols();j++) {
      c[j]+=bi*M[i][j];
    }
  }

  return c ;
}

bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
{
  static unsigned int cpt = 0;
  std::cout << "** Test " << ++cpt << std::endl;
  std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl;
  if(bench.size() != M.size()) {
    std::cout << "Test fails: bad size wrt bench" << std::endl;
    return false;
  }
  for (unsigned int i=0; i<M.size(); i++) {
    if (std::fabs(M.data[i]-bench[i]) > std::fabs(M.data[i])*std::numeric_limits<double>::epsilon()) {
      std::cout << "Test fails: bad content" << std::endl;
      return false;
    }
  }

  return true;
}

// 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();
}

/*!
  \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
lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
{
  if (DEBUG_LEVEL1)
    std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;

  try{
    int i,imin;

    vpMatrix ata ; // A^T A
    ata = a.t()*a ;
    vpMatrix btb ; // B^T B
    btb = b.t()*b ;

    vpMatrix bta ;  // B^T A
    bta = b.t()*a ;

    vpMatrix btb1 ;  // (B^T B)^(-1)

    if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
    else btb1 = btb.pseudoInverse();

    if (DEBUG_LEVEL1)
    {
      std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
      std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
   }

    vpMatrix r ;  // (B^T B)^(-1) B^T A
    r = btb1*bta ;

    vpMatrix e ;  //   - A^T B (B^T B)^(-1) B^T A
    e = - (a.t()*b) *r ;

    e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A

    if (DEBUG_LEVEL1)
    {
      std::cout << " E :" << std::endl << e << std::endl;
    }

    //   vpColVector sv ;
    //    vpMatrix v ;
    e.svd(x1,ata) ;// destructif sur e
    // calcul du vecteur propre de E correspondant a la valeur propre min.

    /* calcul de SVmax	*/
    imin = 0;
    // FC : Pourquoi calculer SVmax ??????
    //     double  svm = 0.0;
    //    for (i=0;i<x1.getRows();i++)
    //    {
    //      if (x1[i] > svm) { svm = x1[i]; imin = i; }
    //    }
    //    svm *= EPS;	/* pour le rang	*/

    for (i=0;i<x1.getRows();i++)
      if (x1[i] < x1[imin]) imin = i;

    if (DEBUG_LEVEL1)
    {
      printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
      std::cout << " i_min " << imin << std::endl;
    }

    for (i=0;i<x1.getRows();i++)
      x1[i] = ata[i][imin];

    x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1

    if (DEBUG_LEVEL1)
    {
      std::cout << " X1 : " <<  x1.t() << std::endl;
      std::cout << " V : " << std::endl << ata << std::endl;
    }
  }
  catch(...)
  {
    vpERROR_TRACE(" ") ;
    throw ;
  }
  if (DEBUG_LEVEL1)
    std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;

}

//! Constructor that take column j of matrix M.
vpColVector::vpColVector (const vpMatrix &M, unsigned int j)
  : vpArray2D<double>(M.getRows(), 1)
{
  for(unsigned int i=0; i< M.getCols(); i++)
    (*this)[i] = M[i][j];
}

/*!
  Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
  
  To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
  
  \param I : The image used to store the result.
  \param cam : The parameters of the virtual camera.
  \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
*/
void
vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters cam, vpMatrix &zBuffer)
{
  if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
    throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
  
  int nb_point_dessine = 0;
  if (cleanPrevImage)
  {
    for (int i = (int)rect.getTop(); i < (int)rect.getBottom(); i++)
    {
      for (int j = (int)rect.getLeft(); j < (int)rect.getRight(); j++)
      {
	I[i][j] = bgColor;
      }
    }
  }
  if(visible)
  {
    getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
    
    double top = rect.getTop();
    double bottom = rect.getBottom();
    double left = rect.getLeft();
    double right= rect.getRight();
    
    vpRGBa *bitmap = I.bitmap;
    unsigned int width = I.getWidth();
    vpImagePoint ip;
    
    for (int i = (int)top; i < (int)bottom; i++)
    {
      for (int j = (int)left; j < (int)right; j++)
      {
        double x=0,y=0;
	ip.set_ij(i,j);
        vpPixelMeterConversion::convertPoint(cam,ip, x,y);
	ip.set_ij(y,x);
	if (colorI == GRAY_SCALED)
	{
	  unsigned char Ipixelplan;
	  if(getPixel(ip,Ipixelplan))
	  {
	    if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
	    {
	      vpRGBa pixelcolor;
	      pixelcolor.R = Ipixelplan;
	      pixelcolor.G = Ipixelplan;
	      pixelcolor.B = Ipixelplan;
	      *(bitmap+i*width+j) = pixelcolor;
	      nb_point_dessine++;
	      zBuffer[i][j] = Xinter_optim[2];
	    }
	  }
	}
	else if (colorI == COLORED)
	{
	  vpRGBa Ipixelplan;
	  if(getPixel(ip,Ipixelplan))
	  {
	    if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
	    {
	      *(bitmap+i*width+j) = Ipixelplan;
	      nb_point_dessine++;
	      zBuffer[i][j] = Xinter_optim[2];
	    }
	  }
	}
      }
    }
  }
}

void
vpMbKltTracker::computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L,
    const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
    vpColVector &R, vpMatrix &LTL, vpColVector &LTR, vpColVector &error_prev, vpColVector &v, double &mu,
    vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) {
  m_error = R;
  if(computeCovariance){
    L_true = L;
    if(!isoJoIdentity){
       vpVelocityTwistMatrix cVo;
       cVo.buildFrom(cMo);
       LVJ_true = (L*cVo*oJo);
    }
  }

  normRes_1 = normRes;
  normRes = 0;
  for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
    w_true[i] = w[i];
    R[i] = R[i] * w[i];
    normRes += R[i];
  }

  if((iter == 0) || compute_interaction){
    for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
      for(unsigned int j=0; j<6; j++){
        L[i][j] *= w[i];
      }
    }
  }

  if(isoJoIdentity){
      LTL = L.AtA();
      computeJTR(L, R, LTR);

      switch(m_optimizationMethod){
      case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
      {
        vpMatrix LMA(LTL.getRows(), LTL.getCols());
        LMA.eye();
        vpMatrix LTLmuI = LTL + (LMA*mu);
        v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;

        if(iter != 0)
          mu /= 10.0;

        error_prev = m_error;
        break;
      }
      case vpMbTracker::GAUSS_NEWTON_OPT:
      default:
        v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
      }
  }
  else{
      vpVelocityTwistMatrix cVo;
      cVo.buildFrom(cMo);
      vpMatrix LVJ = (L*cVo*oJo);
      vpMatrix LVJTLVJ = (LVJ).AtA();
      vpColVector LVJTR;
      computeJTR(LVJ, R, LVJTR);

      switch(m_optimizationMethod){
      case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
      {
        vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
        LMA.eye();
        vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
        v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
        v = cVo * v;

        if(iter != 0)
          mu /= 10.0;

        error_prev = m_error;
        break;
      }
      case vpMbTracker::GAUSS_NEWTON_OPT:
      default:
      {
        v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
        v = cVo * v;
        break;
      }
      }
  }

  cMoPrev = cMo;
  ctTc0_Prev = ctTc0;
  ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
  cMo = ctTc0 * c0Mo;
}