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/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
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
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
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
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
00191 U = u;
00192 V = v;
00193 S = QVMatrix::diagonal(s);
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
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
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
00409 eigVals = eval;
00410 eigVecs = evec;
00411 eigVecs = eigVecs.transpose();
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 gsl_vector_free (eval);
00429 gsl_matrix_free (evec);
00430 }
00431
00432
00433
00434
00435
00436
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
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
00448
00449
00450 return 1.0;
00451 }
00452 if(fabs(e1)/e2 > 0.9)
00453 {
00454
00455
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
00471
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 }