Exemple #1
0
/*!
  \brief Initialisation of a the subColVector
  \param v : parent col vector
  \param offset : offset where subColVector start in the parent colVector
  \param nrows : size of the subColVector
*/
void vpSubColVector::init(vpColVector &v, 
			  const unsigned int & offset, 
			  const unsigned int & nrows){
  
  if(!v.data){
    vpERROR_TRACE("\n\t\t vpSubColvector parent vpColVector has been destroyed");
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t \n\t\t vpSubColvector parent vpColVector has been destroyed")) ;
  }
  
  if(offset+nrows<=v.getRows()){
    data=v.data+offset;
    
    rowNum=nrows;
    colNum = 1;
    
    pRowNum=v.getRows();
    parent=&v;
    
    if(rowPtrs){
      free(rowPtrs);
    }
    
  rowPtrs=(double**)malloc( parent->getRows() * sizeof(double*));
    for(unsigned int i=0;i<nrows;i++)
      rowPtrs[i]=v.data+i+offset;
    
    dsize = rowNum ;
    trsize =0 ;
  }else{
    vpERROR_TRACE("SubColVector cannot be contain in parent ColVector") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"SubColVector cannot be contain in parent ColVector")) ;
  }
}
Exemple #2
0
/*!
  \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")) ;
  }
}
Exemple #3
0
/*!
  \brief This method can be used to detect if the parent colVector
  always exits or its size have not changed and throw an exception is not
*/
void vpSubColVector::checkParentStatus(){
  if(!data){
    vpERROR_TRACE("\n\t\t vpSubColvector parent vpColVector has been destroyed");
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t \n\t\t vpSubColvector parent vpColVector has been destroyed")) ;
  }
  if(pRowNum!=parent->getRows()){
    vpERROR_TRACE("\n\t\t vpSubColvector size of parent vpColVector has been changed");
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"\n\t\t \n\t\t vpSubColvector size of parent vpColVector has been changed")) ;
  }
}
vpColVector
vpColVector::sort(const vpColVector &v)
{
  if (v.data==NULL) {
    vpERROR_TRACE("vpColVector a non initialized") ;
    throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
  }
  vpColVector tab ;
  tab = v ;
  unsigned int nb_permutation = 1 ;
  unsigned int i = 0 ;
  while (nb_permutation !=0 )
  {
    nb_permutation = 0 ;
    for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
    {
      if ((tab[j]<tab[j-1]))
      {
	double tmp = tab[j] ;
	tab[j] = tab[j-1] ;
	tab[j-1] = tmp ;
	nb_permutation++ ;
      }
    }
    i++ ;
  }

  return tab ;
}
Exemple #5
0
vpColVector::vpColVector (vpColVector &m, int r, int nrows)
{
  if (r<0)
  {
    vpERROR_TRACE("\n\t\t Illegal subMatrix operation") ;
    throw(vpMatrixException(vpMatrixException::subMatrixError,
			    "\n\t\t Illegal subMatrix operation")) ;
  }
  if ( (r+nrows) > m.getRows() )
  {
    vpERROR_TRACE("\n\t\t SubvpMatrix larger than vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::subMatrixError,
			    "\n\t\t SubvpMatrix larger than vpColVector")) ;
  }
  init(m, r, 0, nrows, 1);
}
Exemple #6
0
void
vpMatrix::LUDcmp(unsigned int *perm, int& d)
{
  unsigned int n = rowNum;

  unsigned int i,imax=0,j,k;
  double big,dum,sum_,temp;
  vpColVector vv(n);

  d=1;
  for (i=0;i<n;i++) {
    big=0.0;
    for (j=0;j<n;j++)
      if ((temp=fabs(rowPtrs[i][j])) > big) big=temp;
    //if (big == 0.0)
    if (std::fabs(big) <= std::numeric_limits<double>::epsilon())
    {
      //vpERROR_TRACE("Singular vpMatrix in  LUDcmp") ;
      throw(vpMatrixException(vpMatrixException::matrixError,
                              "Singular vpMatrix in  LUDcmp")) ;
    }
    vv[i]=1.0/big;
  }
  for (j=0;j<n;j++) {
    for (i=0;i<j;i++) {
      sum_=rowPtrs[i][j];
      for (k=0;k<i;k++) sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
      rowPtrs[i][j]=sum_;
    }
    big=0.0;
    for (i=j;i<n;i++) {
      sum_=rowPtrs[i][j];
      for (k=0;k<j;k++)
        sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
      rowPtrs[i][j]=sum_;
      if ( (dum=vv[i]*fabs(sum_)) >= big) {
        big=dum;
        imax=i;
      }
    }
    if (j != imax) {
      for (k=0;k<n;k++) {
        dum=rowPtrs[imax][k];
        rowPtrs[imax][k]=rowPtrs[j][k];
        rowPtrs[j][k]=dum;
      }
      d *= -1;
      vv[imax]=vv[j];
    }
    perm[j]=imax;
    //if (rowPtrs[j][j] == 0.0)
    if (std::fabs(rowPtrs[j][j]) <= std::numeric_limits<double>::epsilon())
      rowPtrs[j][j]=TINY;
    if (j != n) {
      dum=1.0/(rowPtrs[j][j]);
      for (i=j+1;i<n;i++) rowPtrs[i][j] *= dum;
    }
  }
}
vpColVector::vpColVector (vpColVector &m, unsigned int r, unsigned int nrows)
{
  if ( (r+nrows) > m.getRows() )
  {
    vpERROR_TRACE("\n\t\t SubvpMatrix larger than vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::subMatrixError,
			    "\n\t\t SubvpMatrix larger than vpColVector")) ;
  }
  init(m, r, 0, nrows, 1);
}
Exemple #8
0
/*!
  \brief Operation A = B 
  \param B : a vpMatrix of size nrow x 1
*/
vpSubColVector & vpSubColVector::operator=(const vpMatrix &B){
  if ((B.getCols()!=1)||(rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpSubColVector mismatch in operator vpSubColVector=vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t \n\t\t vpSubColVector mismatch in operator vpSubColVector=vpMatrix")) ;
  }
  
  for (unsigned int i=0;i<rowNum;i++)
    data[i] = B[i][1];
  return *this;
}
/*!
  \relates vpColVector
  \brief compute  the dot product of two vectors C = a.b
*/
double
vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
{
  if (a.data==NULL)
  {
    vpERROR_TRACE("vpColVector a non initialized") ;
    throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
  }
  if (b.data==NULL)
  {
    vpERROR_TRACE("vpColVector b non initialized") ;
    throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
  }
  double *ad = a.data ;   double *bd = b.data ;

  double c = 0 ;
  for (unsigned int i=0 ; i < a.getRows() ; i++)
    c += *(ad++)* *(bd++) ;
  //  vpMatrix c = (a.t() * b);
  //  return c[0][0];
  return c ;
}
Exemple #10
0
//! Divide  all the element of the homography matrix by v : Hij = Hij / v
vpHomography & vpHomography::operator/=(double v)
{
  //if (x == 0)
  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
    throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");

  double vinv = 1/v ;

  for (unsigned int i=0;i<9;i++)
    data[i] *= vinv;

  return *this;
}
Exemple #11
0
vpMatrix
vpMatrix::inverseByQR() const
{

  if ( rowNum != colNum)
  {
    vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::matrixError,
                            "Cannot invert a non-square vpMatrix")) ;
  }
#ifdef VISP_HAVE_LAPACK_C
  return inverseByQRLapack();
#endif
}
/*!
  \brief Compute the cross product of two vectors C = a x b

  \param a : vpColVector
  \param b : vpColVector
*/
vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b)
{

  if (a.getRows() != 3)
  {
    vpERROR_TRACE("Cannot compute cross product,"
		"a has not 3 rows") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t Cannot compute  cross product"
			    "a has not 3 rows")) ;
  }
  if (b.getRows() != 3)
  {

    vpERROR_TRACE("Cannot compute cross product,"
		"b has not 3 rows") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t Cannot compute  cross product"
			    "b has not 3 rows")) ;;
  }

  return vpColVector::skew(a) * b;
}
Exemple #13
0
/*!
 
  Divide an homography by a scalar.

  \param v : Value of the scalar.

  \code 
  vpHomography aHb; 
  // Initialize aHb
  vpHomography H = aHb / aHb[2][2];  
  \endcode

*/
vpHomography vpHomography::operator/(const double &v) const
{
  vpHomography H;
  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
    throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");

  double vinv = 1/v ;

  for (unsigned int i=0; i < 9; i ++) {
    H.data[i] = this->data[i] * vinv;
  }

  return H;
}
/*!
  Compute the median value of all the element of the vector
*/
double
vpColVector::median(const vpColVector &v)
{
  if (v.data==NULL)
  {
    vpERROR_TRACE("vpColVector v non initialized") ;
    throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
  }

  unsigned int i,j;
  unsigned int inf, sup;
  unsigned int n = v.getRows() ;
  vpColVector infsup(n) ;

  for (i=0;i<v.getRows();i++)
  {
    // We compute the number of element gretear (sup) than the current
    // value and the number of element smaller (inf) than the current
    // value
    inf = sup = 0;
    for (j=0;j<v.getRows();j++)
    {
      if (i != j)
      {
        if (v[i] <= v[j]) inf++;
        if (v[i] >= v[j]) sup++;
      }
    }
    // We compute then difference between inf and sup
    // the median should be for |inf-sup| = 0 (1 if an even number of element)
    // which means that there are the same number of element in the array
    // that are greater and smaller that this value.
    infsup[i] = fabs((double)(inf-sup)); //EM gcc 4.3
  }

  // seek for the smaller value of |inf-sup| (should be 0 or 1)
  unsigned int imin=0 ; // index of the median in the array
  // min cannot be greater than the number of element
  double  min = v.getRows();
  for (i=0;i<v.getRows();i++)
    if (infsup[i] < min)
    {
      min = infsup[i];
      imin = i ;
    }

  // return the median
  return(v[imin]);

}
Exemple #15
0
/*!
  \brief Operation A = B
  \param B : a matrix
*/
vpSubMatrix & vpSubMatrix::operator=(const vpMatrix &B){
  
  if ((colNum != B.getCols())||(rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
  }
  
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j] = B[i][j];
    
    return *this;
}
Exemple #16
0
/*!
  Compute the inverse of a n-by-n matrix using the LU decomposition.

  \return The inverse matrix.

  Here an example:
  \code
#include <visp3/core/vpMatrix.h>

int main()
{
  vpMatrix A(4,4);

  A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
  A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
  A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
  A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;

  // Compute the inverse
  vpMatrix A_1; // A^-1
  A_1 = A.inverseByLU();
  std::cout << "Inverse by LU: \n" << A_1 << std::endl;

  std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
}
  \endcode

  \sa pseudoInverse()
*/
vpMatrix
vpMatrix::inverseByLU() const
{
  unsigned int i,j;

  if ( rowNum != colNum)
  {
    vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::matrixError,
			    "Cannot invert a non-square vpMatrix")) ;
  }

  vpMatrix B(rowNum, rowNum), X(rowNum, rowNum);
  vpMatrix V(rowNum, rowNum);
  vpColVector W(rowNum);

  for (i=0; i<rowNum; i++) {
    for (j=0; j<rowNum; j++) {
      B[i][j] = (i == j) ? 1 : 0;
    }
  }

  vpMatrix A(rowNum, rowNum);
  A = *this;

  unsigned int *perm = new unsigned int[rowNum];
  int p;

  try {
    A.LUDcmp(perm, p);
  }
  catch(vpException e) {
    delete [] perm;
    throw(e);
  }

  vpColVector c_tmp(rowNum)  ;
  for (j=1; j<=rowNum; j++)
  {
    c_tmp =0 ;  c_tmp[j-1] = 1 ;
    A.LUBksb(perm, c_tmp);
    for (unsigned int k=0 ; k < c_tmp.getRows() ; k++)
      B[k][j-1] = c_tmp[k] ;
  }
  delete [] perm;
  return B;
}
/*!
  \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];
}
/*!
  \brief Compute the mean value of all the element of the vector
*/
double vpColVector::mean(const vpColVector &v)
{
 if (v.data==NULL)
  {
    vpERROR_TRACE("vpColVector v non initialized") ;
    throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
  }
 double mean = 0 ;
  double *vd = v.data ;
  for (unsigned int i=0 ; i < v.getRows() ; i++)
    mean += *(vd++) ;

  /*
  for (int i=0 ; i < v.getRows() ; i++)
  {
    mean += v[i] ;
  }
   mean /= v.rowNum ;
   return mean;
  */
  return mean/v.getRows();
}
/*!
  Compute the skew symmetric matrix \f$[{\bf v}]_\times\f$ of vector v.

  \f[ \mbox{if} \quad  {\bf V} =  \left( \begin{array}{c} x \\ y \\  z
  \end{array}\right), \quad \mbox{then} \qquad
  [{\bf v}]_\times = \left( \begin{array}{ccc}
  0 & -z & y \\
  z & 0 & -x \\
  -y & x & 0
  \end{array}\right)
  \f]

  \param v : Input vector used to compute the skew symmetric matrix.
*/
vpMatrix
vpColVector::skew(const vpColVector &v)
{

  vpMatrix M ;
  if (v.getRows() != 3)
  {
    vpERROR_TRACE("Cannot compute skew kinematic matrix,"
		"v has not 3 rows") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t Cannot compute skew kinematic matrix"
			    "v has not 3 rows")) ;
  }


  M.resize(3,3) ;
  M[0][0] = 0 ;     M[0][1] = -v[2] ;  M[0][2] = v[1] ;
  M[1][0] = v[2] ;  M[1][1] = 0 ;      M[1][2] = -v[0] ;
  M[2][0] = -v[1] ; M[2][1] = v[0] ;   M[2][2] = 0 ;


  return M ;
}
Exemple #20
0
/*!
  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];
	    }
	  }
	}
      }
    }
  }
}