PARP Research Group University of Murcia, Spain


src/qvmath/qvmatrixalgebra.cpp

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



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