00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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
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
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
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
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
00192 U = u;
00193 V = v;
00194 S = QVMatrix::diagonal(s);
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
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
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
00410 eigVals = eval;
00411 eigVecs = evec;
00412 eigVecs = eigVecs.transpose();
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429 gsl_vector_free (eval);
00430 gsl_matrix_free (evec);
00431 }
00432
00433
00434
00435
00436
00437
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
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
00449
00450
00451 return 1.0;
00452 }
00453 if(fabs(e1)/e2 > 0.9)
00454 {
00455
00456
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
00472
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 }