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, S, V);
00136
00137
00138 x = V.getCol(V.getCols()-1);
00139 }
00140
00141 void solveHomogeneousLinear2(const QVMatrix &A, QVector<double> &x)
00142 {
00143 const int rows = A.getRows(), cols = A.getCols();
00144
00145
00146 QVMatrix A2(rows, cols-1);
00147
00148 for (int i = 1; i < cols; i++)
00149 A2.setCol(i-1, A.getCol(i));
00150
00151
00152 double minValue = std::numeric_limits<double>::max();
00153 QVVector bestX(cols);
00154 for (int i = 0; i <= cols-1; i++)
00155 {
00156 QVVector b = A.getCol(i);
00157 QVMatrix bMat(rows, 1);
00158 bMat.setCol(0,b);
00159
00160 QVMatrix x_temp = bMat.transpose() / A2.transpose();
00161
00162 if (x_temp.norm2() < minValue)
00163 {
00164 bestX = x_temp.getRow(0);
00165 bestX.insert(i,-1);
00166 minValue = x_temp.norm2();
00167 }
00168
00169
00170 if (i < cols - 1)
00171 A2.setCol(i, A.getCol(i));
00172 }
00173
00174 x = bestX;
00175 }
00176
00177 void SingularValueDecomposition(const QVMatrix &M, QVMatrix &U, QVMatrix &S, QVMatrix &V)
00178 {
00179 const int 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 gsl_matrix *X = gsl_matrix_alloc (dim2,dim2);
00187 gsl_linalg_SV_decomp_mod(u, X, v, s, work);
00188 gsl_matrix_free(X);
00189
00190
00191 U = u;
00192 V = v;
00193 S = QVMatrix::diagonal(s);
00194
00195 gsl_matrix_free(u);
00196 gsl_vector_free(s);
00197 gsl_matrix_free(v);
00198 gsl_vector_free(work);
00199 }
00200
00201 void singularValueDecomposition(const QVMatrix &M, QVMatrix &U, QVMatrix &V, QVMatrix &S)
00202 {
00203 std::cout << "DEPRECATED: singularValueDecomposition. Use SingularValueDecomposition or SVD instead." << std::endl;
00204 const int dim2 = M.getCols();
00205
00206 gsl_matrix *u = M;
00207 gsl_vector *s = gsl_vector_alloc (dim2);
00208 gsl_matrix *v = gsl_matrix_alloc (dim2, dim2);
00209 gsl_vector *work = gsl_vector_alloc (dim2);
00210
00211 gsl_matrix *X = gsl_matrix_alloc (dim2,dim2);
00212 gsl_linalg_SV_decomp_mod(u, X, v, s, work);
00213 gsl_matrix_free(X);
00214
00215 U = u;
00216 V = v;
00217 S = QVMatrix::diagonal(s);
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, S, V);
00347
00348
00349 const int dim2 = M.getCols();
00350
00351 double *dataBufferS = S.getWriteData();
00352 for (int i = 0; i < dim2; i++)
00353 dataBufferS[i*dim2+i] = 1/dataBufferS[i*dim2+i];
00354
00355 return V * S * U.transpose();
00356 }
00357
00358 double determinant(const QVMatrix &M)
00359 {
00360 Q_ASSERT(M.getRows() == M.getCols());
00361 Q_ASSERT(M.getRows() > 1);
00362
00363 QVMatrix U, V, S;
00364
00366 SingularValueDecomposition(M, U, S, V);
00367
00368
00369 double value=1.0;
00370 const int dim = M.getRows();
00371
00372 double *dataBufferS = S.getWriteData();
00373 for (int i = 0; i < dim; i++)
00374 value *= dataBufferS[i*dim+i];
00375
00376 return value;
00377 }
00378
00379 void eigenDecomposition(const QVMatrix &M, QVVector &eigVals, QVMatrix &eigVecs)
00380 {
00381
00382 Q_ASSERT(M.getCols() == M.getRows());
00383
00384 double data[M.getDataSize()];
00385
00386 const double *dataBuffer = M.getReadData();
00387 for(int i = 0; i < M.getDataSize(); i++)
00388 data[i] = dataBuffer[i];
00389
00390 const int dim = M.getRows();
00391 gsl_matrix_view m = gsl_matrix_view_array (data, dim, dim);
00392 gsl_vector *eval = gsl_vector_alloc (dim);
00393 gsl_matrix *evec = gsl_matrix_alloc (dim, dim);
00394
00395 gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (dim);
00396 gsl_eigen_symmv (&m.matrix, eval, evec, w);
00397 gsl_eigen_symmv_free (w);
00398 gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_DESC);
00399
00401
00402
00403 eigVals = eval;
00404 eigVecs = evec;
00405 eigVecs = eigVecs.transpose();
00406
00407 gsl_vector_free (eval);
00408 gsl_matrix_free (evec);
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 }