src/qvmath/qvmatrixalgebra.cpp

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

Generated on Thu Jul 17 17:23:28 2008 for QVision by  doxygen 1.5.3