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