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
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
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
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
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
00167 U = u;
00168 V = v;
00169 S = QVMatrix::diagonal(s);
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
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
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
00385 eigVals = eval;
00386 eigVecs = evec;
00387 eigVecs = eigVecs.transpose();
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404 gsl_vector_free (eval);
00405 gsl_matrix_free (evec);
00406 }
00407
00408
00409
00410
00411
00412
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
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
00424
00425
00426 return 1.0;
00427 }
00428 if(fabs(e1)/e2 > 0.9)
00429 {
00430
00431
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
00447
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 }