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