PARP Research Group University of Murcia, Spain


src/qvmath/qvmatrixalgebra.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007, 2008, 2009. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #include <gsl/gsl_math.h>
00026 #include <gsl/gsl_eigen.h>
00027 #include <gsl/gsl_linalg.h>
00028 
00029 #include <qvmath.h>
00030 #include <qvdefines.h>
00031 #include <qvmath/qvmatrixalgebra.h>
00032 
00033 void solveLinear(const QVMatrix &A, QVVector &x, const QVVector &b)
00034         {
00035         Q_ASSERT(A.getCols() == A.getRows());
00036         Q_ASSERT(A.getCols() == x.size());
00037         Q_ASSERT(A.getCols() == b.size());
00038 
00039         gsl_matrix *gA = A;
00040         gsl_vector *gB = b;
00041 
00042         gsl_linalg_HH_svx(gA, gB);
00043         x = gB;
00044 
00045         gsl_matrix_free(gA);
00046         gsl_vector_free(gB);
00047         }
00048 
00049 void solveLinear(const QVMatrix &A, QVMatrix &X, const QVMatrix &B)
00050         {
00051         Q_ASSERT(A.getCols() == A.getRows());
00052         Q_ASSERT(A.getCols() == X.getRows());
00053         Q_ASSERT(A.getCols() == B.getRows());
00054 
00055         const int dimN = A.getRows();
00056         const int numS = X.getCols();
00057         int signum;
00058 
00059         double *dataX = X.getWriteData();
00060         const double *dataB = B.getReadData();
00061 
00062         gsl_matrix *a = A;
00063         gsl_permutation *p = gsl_permutation_alloc(dimN);
00064         gsl_vector *b = gsl_vector_alloc(dimN);
00065         gsl_vector *x = gsl_vector_alloc(dimN);
00066 
00067         gsl_linalg_LU_decomp(a, p, &signum);
00068 
00069         for(int sist = 0; sist < numS; sist++)
00070                 {
00071                 for(int i = 0; i < dimN; i++)
00072                         b->data[i] = dataB[i*numS + sist];
00073 
00074                 gsl_linalg_LU_solve(a, p, b, x);
00075 
00076                 for(int i = 0; i < dimN; i++)
00077                         dataX[i*numS + sist] = x->data[i];
00078                 }
00079 
00080         gsl_matrix_free(a);
00081         gsl_permutation_free(p);
00082         gsl_vector_free(b);
00083         gsl_vector_free(x);
00084         }
00085 
00086 void solveOverDetermined(const QVMatrix &A, QVMatrix &X, const QVMatrix &B)
00087         {
00088         Q_ASSERT(A.getCols() <= A.getRows());
00089         Q_ASSERT(A.getCols() == X.getRows());
00090         Q_ASSERT(A.getRows() == B.getRows());
00091 
00092         const int dim1 = A.getRows();
00093         const int dim2 = A.getCols();
00094         const int numS = X.getCols();
00095 
00096         double *dataX = X.getWriteData();
00097         const double *dataB = B.getReadData();
00098 
00099         gsl_matrix *u = A;
00100         gsl_vector *s = gsl_vector_alloc(dim2);
00101         gsl_matrix *v = gsl_matrix_alloc(dim2, dim2);
00102         gsl_vector *workV = gsl_vector_alloc(dim2);
00103         gsl_matrix *workM = gsl_matrix_alloc(dim2,dim2);
00104         gsl_vector *b = gsl_vector_alloc(dim1);
00105         gsl_vector *x = gsl_vector_alloc(dim2);
00106 
00107         gsl_linalg_SV_decomp_mod(u, workM, v, s, workV);
00108 
00109         for(int sist = 0; sist < numS; sist++)
00110                 {
00111                 for(int i = 0; i < dim1; i++)
00112                         b->data[i] = dataB[i*numS + sist];
00113 
00114                 gsl_linalg_SV_solve(u, v, s, b, x);
00115 
00116                 for(int i = 0; i < dim2; i++)
00117                         dataX[i*numS + sist] = x->data[i];
00118                 }
00119 
00120         gsl_matrix_free(u);
00121         gsl_vector_free(s);
00122         gsl_matrix_free(v);
00123         gsl_vector_free(workV);
00124         gsl_matrix_free(workM);
00125         gsl_vector_free(b);
00126         gsl_vector_free(x);
00127         }
00128 
00129 void solveHomogeneousLinear(const QVMatrix &A, QVector<double> &x)
00130         {
00131         QVMatrix U, V, S;
00132         singularValueDecomposition(A, U, V, S);
00133 
00134         x = V.getCol(V.getCols()-1);
00135         }
00136 
00137 void solveHomogeneousLinear2(const QVMatrix &A, QVector<double> &x)
00138         {
00139         const int rows = A.getRows(), cols = A.getCols();
00140 
00141         // Initialize A2 matrix and b vector.
00142         QVMatrix A2(rows, cols-1);
00143 
00144         for (int i = 1; i < cols; i++)
00145                 A2.setCol(i-1, A.getCol(i));
00146 
00147         // Find the best solution
00148         double minValue = std::numeric_limits<double>::max();
00149         QVVector bestX(cols);
00150         for (int i = 0; i <= cols-1; i++)
00151                 {
00152                 QVVector b = A.getCol(i);
00153                 QVMatrix bMat(rows, 1);
00154                 bMat.setCol(0,b);
00155 
00156                 QVMatrix x_temp =  bMat.transpose() / A2.transpose();
00157 
00158                 if (x_temp.norm2() < minValue)
00159                         {
00160                         bestX = x_temp.getRow(0);
00161                         bestX.insert(i,-1);
00162                         minValue = x_temp.norm2();                      
00163                         }
00164 
00165                 // Swap actual column
00166                 if (i < cols - 1)
00167                         A2.setCol(i, A.getCol(i));
00168                 }
00169 
00170         x = bestX;
00171         }
00172 
00173 void singularValueDecomposition(const QVMatrix &M, QVMatrix &U, QVMatrix &V, QVMatrix &S)
00174         {
00175         const int       /*dim1 = M.getRows(),*/
00176                         dim2 = M.getCols();
00177 
00178         gsl_matrix *u = M;
00179         gsl_vector *s = gsl_vector_alloc (dim2);
00180         gsl_matrix *v = gsl_matrix_alloc (dim2, dim2);
00181         gsl_vector *work = gsl_vector_alloc (dim2);
00182 
00183         //gsl_linalg_SV_decomp(u, v, s, work);
00184 
00185         gsl_matrix *X = gsl_matrix_alloc (dim2,dim2);
00186         gsl_linalg_SV_decomp_mod(u, X, v, s, work);
00187         gsl_matrix_free(X);
00188 
00190 
00191         // Esto:
00192         U = u;
00193         V = v;
00194         S = QVMatrix::diagonal(s);
00195 
00196         // ... es equivalente a esto:
00197         /*U = QVMatrix(dim1, dim2);
00198         V = QVMatrix(dim2, dim2);
00199         S = QVMatrix(dim2, dim2);
00200 
00201         double  *dataU = U.getWriteData(),
00202                 *dataV = V.getWriteData(),
00203                 *dataS = S.getWriteData();
00204 
00205         for (int i = 0; i < dim1; i++)
00206                 for (int j = 0; j < dim2; j++)
00207                         dataU[i*dim2 + j] = u->data[i*dim2+j];
00208 
00209         for (int i = 0; i < dim2; i++)
00210                 {
00211                 for (int j = 0; j < dim2; j++)
00212                         {
00213                         dataV[i*dim2 + j] = v->data[i*dim2+j];
00214                         dataS[i*dim2 + j] = 0;
00215                         }
00216                 dataS[i*dim2+i] = s->data[i];
00217                 }*/
00218 
00219         gsl_matrix_free(u);
00220         gsl_vector_free(s);
00221         gsl_matrix_free(v);
00222         gsl_vector_free(work);
00223         }
00224 
00225 void LUDecomposition(const QVMatrix &M, QVMatrix &L, QVMatrix &U, QVMatrix &P)
00226         {
00227         const int       dim1 = M.getRows(),
00228                         dim2 = M.getCols();
00229 
00230         gsl_matrix *a = M;
00231         gsl_permutation *p = gsl_permutation_alloc(dim2);
00232 
00233         int signum;
00234 
00235 
00236         gsl_linalg_LU_decomp(a, p, &signum);
00237 
00238 
00239         L = QVMatrix(dim1, dim2);
00240         U = QVMatrix(dim1, dim2);
00241         P = QVMatrix(dim1, dim2);
00242 
00243         double  *dataL = L.getWriteData(),
00244                 *dataU = U.getWriteData(),
00245                 *dataP = P.getWriteData();
00246 
00247         for (int i = 0; i < dim1; i++)
00248                 for (int j = 0; j < dim2; j++)
00249                         {
00250                         if (j > i)
00251                                 {
00252                                 dataU[i*dim2 + j] = a->data[i*dim2+j];
00253                                 dataL[i*dim2 + j] = 0;
00254                                 }
00255                         else if (j < i)
00256                                 {
00257                                 dataU[i*dim2 + j] = 0;
00258                                 dataL[i*dim2 + j] = a->data[i*dim2+j];
00259                                 }
00260                         else
00261                                 {
00262                                 dataU[i*dim2 + j] = a->data[i*dim2+j];
00263                                 dataL[i*dim2 + j] = 1;
00264                                 }
00265                         }
00266 
00267         for (int i = 0; i < dim1; i++)
00268                 for (int j = 0; j < dim2; j++)
00269                         dataP[i*dim2 + j] = 0;
00270 
00271         for (int j = 0; j < dim2; j++)
00272                 dataP[(p->data[j])*dim2 + j] = 1;
00273 
00274 
00275         gsl_matrix_free(a);
00276         gsl_permutation_free(p);
00277         }
00278 
00279 void CholeskyDecomposition(const QVMatrix &M, QVMatrix &L)
00280         {
00281         const int       dim1 = M.getRows(),
00282                         dim2 = M.getCols();
00283 
00284         gsl_matrix *a = M;
00285 
00286 
00287         gsl_linalg_cholesky_decomp(a);
00288 
00289 
00290         L = QVMatrix(dim1, dim2);
00291         double  *dataL = L.getWriteData();
00292 
00293         for (int i = 0; i < dim1; i++)
00294                 for (int j = 0; j < dim2; j++)
00295                         {
00296                         if (j <= i)
00297                                 dataL[i*dim2 + j] = a->data[i*dim2+j];
00298                         else
00299                                 dataL[i*dim2 + j] = 0;
00300                         }
00301 
00302         gsl_matrix_free(a);
00303         }
00304 
00305 void QRDecomposition(const QVMatrix &M, QVMatrix &Q, QVMatrix &R)
00306         {
00307         const int       dim1 = M.getRows(),
00308                         dim2 = M.getCols(),
00309                         min = (dim1<dim2 ? dim1: dim2);
00310 
00311         gsl_matrix *a = M;
00312         gsl_matrix *q = gsl_matrix_alloc(dim1, dim1);
00313         gsl_matrix *r = gsl_matrix_alloc(dim1, dim2);
00314         gsl_vector *tau = gsl_vector_alloc(min);
00315 
00316         gsl_linalg_QR_decomp(a, tau);
00317         gsl_linalg_QR_unpack (a, tau, q, r);
00318 
00319         Q = QVMatrix(dim1, dim2);
00320         R = QVMatrix(dim1, dim2);
00321 
00322         double  *dataQ = Q.getWriteData(),
00323                 *dataR = R.getWriteData();
00324 
00325         for (int i = 0; i < dim1; i++)
00326                 for (int j = 0; j < dim1; j++)
00327                         dataQ[i*dim2 + j] = q->data[i*dim2+j];
00328 
00329         for (int i = 0; i < dim1; i++)
00330                 for (int j = 0; j < dim2; j++)
00331                         dataR[i*dim2 + j] = r->data[i*dim2+j];
00332 
00333         gsl_matrix_free(a);
00334         gsl_matrix_free(q);
00335         gsl_matrix_free(r);
00336         gsl_vector_free(tau);
00337         }
00338 
00339 QVMatrix pseudoInverse(const QVMatrix &M)
00340         {
00341         if (M.getRows() < M.getCols())
00342                 return pseudoInverse(M.transpose()).transpose();
00343 
00344         QVMatrix U, V, S;
00345 
00346         singularValueDecomposition(M, U, V, S);
00347 
00348         const int dim2 = M.getCols();
00349 
00350         double *dataBufferS = S.getWriteData();
00351         for (int i = 0; i < dim2; i++)
00352                 dataBufferS[i*dim2+i] = 1/dataBufferS[i*dim2+i];
00353 
00354         return V * S * U.transpose();
00355         }
00356 
00357 double determinant(const QVMatrix &M)
00358         {
00359         Q_ASSERT(M.getRows() == M.getCols());
00360         Q_ASSERT(M.getRows() > 1);
00361 
00362         QVMatrix U, V, S;
00363 
00365         singularValueDecomposition(M, U, V, S);
00366 
00367         double value=1.0;
00368         const int dim = M.getRows();
00369 
00370         double *dataBufferS = S.getWriteData();
00371         for (int i = 0; i < dim; i++)
00372                 value *= dataBufferS[i*dim+i];
00373 
00374         return value;
00375         }
00376 
00377 double BhattacharyyaDistance(const QVVector &m1, const QVMatrix &S1, const QVVector &m2, const QVMatrix &S2)
00378         {
00379         QVVector diff = m2-m1;
00380         QVMatrix mS = (S1+S2)/2;
00381         QVMatrix inv = mS.inverse();
00382         double value = (diff*(inv*diff))/8 + log(mS.det()/sqrt(S1.det()*S2.det()))/2;
00383         return value;
00384         }
00385 
00386 void eigenDecomposition(const QVMatrix &M, QVVector &eigVals, QVMatrix &eigVecs)
00387         {
00388         // Check matrix M is symetric
00389         Q_ASSERT(M.getCols() == M.getRows());
00390 
00391         double data[M.getDataSize()];
00392 
00393         const double *dataBuffer = M.getReadData();
00394         for(int i = 0; i < M.getDataSize(); i++)
00395                 data[i] = dataBuffer[i];
00396         
00397         const int dim = M.getRows();
00398         gsl_matrix_view m = gsl_matrix_view_array (data, dim, dim);
00399         gsl_vector *eval = gsl_vector_alloc (dim);
00400         gsl_matrix *evec = gsl_matrix_alloc (dim, dim);
00401 
00402         gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (dim);
00403         gsl_eigen_symmv (&m.matrix, eval, evec, w);
00404         gsl_eigen_symmv_free (w);
00405         gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_DESC);
00406 
00408 
00409         // Esto:
00410         eigVals = eval;
00411         eigVecs = evec;
00412         eigVecs = eigVecs.transpose();
00413 
00414         // ... es equivalente a esto:
00415         /*
00416         eigVals = QVector<double>(dim);
00417         eigVecs = QVMatrix(dim, dim);
00418 
00419         double  *dataVecs = eigVecs.getWriteData();
00420 
00421         for (int i = 0; i < dim; i++)
00422                 {
00423                 eigVals[i] = eval->data[i];
00424                 for (int j = 0; j < dim; j++)
00425                         dataVecs[i*dim + j] = evec->data[j*dim + i];
00426                 }
00427         */
00428 
00429         gsl_vector_free (eval);
00430         gsl_matrix_free (evec);
00431         }
00432 
00433 // Adjust a line to given first and second order moments, and returns ratio of
00434 // eigenvalues:
00435 
00436 // Adjust a line to given first and second order moments, and returns ratio of 
00437 // eigenvalues: 
00438 double homogLineFromMoments(double x,double y,double xx,double xy,double yy,double &a,double &b,double &c)
00439         {
00440         double a11=xx-x*x, a12=xy-x*y, a22=yy-y*y, temp, e1, e2, angle, cosangle, sinangle;
00441 
00442         // Validity check:
00443         temp = sqrt(a11*a11+4*a12*a12-2*a11*a22+a22*a22);
00444         e1 = a11+a22-temp;
00445         e2 = a11+a22+temp;
00446         if(e2<EPSILON)
00447                 {
00448                 /*std::cerr << " a11=" << a11 << " a12=" << a12 << " a22=" << a22  
00449                 << " x=" << x << " y=" << y << " xx=" << xx << " xy=" << xy << " yy=" << yy  
00450                 << ": Imposible moments in homogLineFromMoments!\n";*/ 
00451                 return 1.0; 
00452                 } 
00453         if(fabs(e1)/e2 > 0.9)
00454                 {
00455                 /*std::cerr << "Too high ratio of eigenvalues e1=" << e1 << "e2=" << e2 <<": No principal direction in homogLineFromMoments!\n"; 
00456                 std::cerr << " a11=" << a11 << " a12=" << a12 << " a22=" << a22 << "\n";*/ 
00457                 return fabs(e1)/e2;
00458                 } 
00459 
00460         if(fabs(a12)>EPSILON)
00461                 angle = atan2(2*a12,a11-a22+temp);
00462         else
00463         if(a11>=a22)
00464                 angle = 0;
00465         else
00466                 angle = PI/2;
00467         if(angle < 0)
00468                 angle += PI;
00469         cosangle = cos(angle); sinangle = sin(angle);
00470 
00471         //Standard deviation in perpendicular direction:
00472         //desv_perp = sqrt(fabs(a11+a22-temp)/2.0);
00473         a = -sinangle; b = cosangle; c = x*sinangle-y*cosangle;
00474         return fabs(e1)/e2;
00475         }
00476 
00477 QVVector regressionLine(const QVMatrix &points)
00478         {
00480         double x = 0, y = 0, xx = 0, yy = 0, xy = 0;
00481         const int rows = points.getRows();
00482 
00483         for (int i = 0; i < rows; i++)
00484                 {
00485                 double xActual = points(i,0), yActual = points(i,1);
00486                 x += xActual;
00487                 y += yActual;
00488                 xx += xActual*xActual;
00489                 xy += xActual*yActual;
00490                 yy += yActual*yActual;
00491                 }
00492 
00493         x /= rows; y /= rows; xx /= rows; xy /= rows; yy /= rows;
00494 
00495         double a, b, c;
00496         if (homogLineFromMoments(x,y,xx,xy,yy,a,b,c))
00497                 {               
00498                 QVVector result(3);
00499                 result[0] = a; result[1] = b; result[2] = c;
00500                 return result;
00501                 }
00502         else
00503                 return QVVector();
00504         }



QVision framework. PARP research group, copyright 2007, 2008.