42 #include <visp/vpConfig.h>
44 #ifndef DOXYGEN_SHOULD_SKIP_THIS
46 #include <visp/vpMatrix.h>
47 #include <visp/vpMath.h>
48 #include <visp/vpColVector.h>
49 #include <visp/vpException.h>
50 #include <visp/vpMatrixException.h>
51 #include <visp/vpDebug.h>
65 static double pythag(
double a,
double b)
70 if (absa > absb)
return absa*sqrt(1.0+
vpMath::sqr(absb/absa));
72 else return (std::fabs(absb) <= std::numeric_limits<double>::epsilon() ? 0.0 : absb*sqrt(1.0+
vpMath::sqr(absa/absb)));
78 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
100 #define MAX_ITER_SVD 50
106 double epsilon = 10*std::numeric_limits<double>::epsilon();
108 unsigned int flag,i,its,j,jj,k,l=0,nm=0;
109 double c,f,h,s,x,y,z;
110 double anorm=0.0,g=0.0,scale=0.0;
114 double **a =
new double*[m+1];
115 double **v =
new double*[n+1];
119 double *w =
new double[n+1];
120 for (i=0;i<n;i++) w[i+1] = 0.0;
134 vpERROR_TRACE(
"\n\t\tSVDcmp: You must augment A with extra zero rows") ;
136 "\n\t\tSVDcmp: You must augment A with "
137 "extra zero rows")) ;
139 double* rv1=
new double[n+1];
146 for (k=i;k<=m;k++) scale += fabs(a[k][i]);
148 if ((std::fabs(scale) > epsilon)) {
151 s += a[k][i]*a[k][i];
154 g = -SIGN(sqrt(s),f);
159 for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
161 for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
164 for (k=i;k<=m;k++) a[k][i] *= scale;
169 if (i <= m && i != n) {
170 for (k=l;k<=n;k++) scale += fabs(a[i][k]);
172 if ((std::fabs(scale) > epsilon) ) {
175 s += a[i][k]*a[i][k];
178 g = -SIGN(sqrt(s),f);
181 for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
184 for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
185 for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
188 for (k=l;k<=n;k++) a[i][k] *= scale;
196 if ((std::fabs(g) > epsilon) ) {
198 v[j][i]=(a[i][j]/a[i][l])/g;
200 for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
201 for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
204 for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
214 for (j=l;j<=n;j++) a[i][j]=0.0;
216 if ((std::fabs(g) > epsilon) ) {
220 for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
222 for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
225 for (j=i;j<=m;j++) a[j][i] *= g;
227 for (j=i;j<=m;j++) a[j][i]=0.0;
232 for (its=1;its<=MAX_ITER_SVD;its++) {
237 if ((std::fabs(rv1[l]) <= epsilon) ) {
242 if ((std::fabs(w[nm]) <= epsilon) )
break;
250 if ((std::fabs(f) > epsilon) ) {
270 for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
274 if (its == MAX_ITER_SVD)
276 for (i=0;i<n;i++) W[i] = w[i+1];
279 std::cout << *
this <<std::endl ;
288 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
290 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
292 for (j=l;j<=nm;j++) {
306 for (jj=1;jj<=n;jj++) {
315 if ((std::fabs(z) > epsilon) ) {
322 for (jj=1;jj<=m;jj++) {
334 for (i=0;i<n;i++) W[i] = w[i+1];
368 unsigned int m = this->
rowNum;
369 unsigned int n = this->
colNum;
379 if (std::fabs(w[j]) > std::numeric_limits<double>::epsilon())
381 for (i=0;i<m;i++) s += u[i][j]*b[i];
388 for (jj=0;jj<n;jj++) s += v[j][jj]*tmp[jj];
415 #define TOLERANCE 1.0e-7
418 void svd_internal_use(
double *U,
double *S,
double *V,
419 unsigned int nRow,
unsigned int nCol)
421 unsigned int i, j, k, EstColRank, RotCount, SweepCount, slimit;
422 double eps, e2, tol, vt, p, x0, y0, q, r, c0, s0, d1, d2;
429 e2 = 10.0 * nRow * eps * eps;
433 for (i = 0; i < nCol; i++)
434 for (j = 0; j < nCol; j++) {
435 V[nCol * i + j] = 0.0;
436 V[nCol * i + i] = 1.0;
438 RotCount = EstColRank * (EstColRank - 1) / 2;
439 while (RotCount != 0 && SweepCount <= slimit) {
440 RotCount = EstColRank * (EstColRank - 1) / 2;
442 for (j = 0; j < EstColRank - 1; j++) {
443 for (k = j + 1; k < EstColRank; k++) {
445 for (i = 0; i < nRow; i++) {
446 x0 = U[nCol * i + j];
447 y0 = U[nCol * i + k];
455 if (q <= e2 * S[0] || fabs(p) <= tol * q)
460 vt = sqrt(4 * p * p + r * r);
461 c0 = sqrt(fabs(.5 * (1 + r / vt)));
463 for (i = 0; i < nRow; i++) {
464 d1 = U[nCol * i + j];
465 d2 = U[nCol * i + k];
466 U[nCol * i + j] = d1 * c0 + d2 * s0;
467 U[nCol * i + k] = -d1 * s0 + d2 * c0;
470 for (i = 0; i < nCol; i++) {
471 d1 = V[nCol * i + j];
472 d2 = V[nCol * i + k];
473 V[nCol * i + j] = d1 * c0 + d2 * s0;
474 V[nCol * i + k] = -d1 * s0 + d2 * c0;
481 vt = sqrt(4 * p * p + q * q);
482 s0 = sqrt(fabs(.5 * (1 - q / vt)));
486 for (i = 0; i < nRow; i++) {
487 d1 = U[nCol * i + j];
488 d2 = U[nCol * i + k];
489 U[nCol * i + j] = d1 * c0 + d2 * s0;
490 U[nCol * i + k] = -d1 * s0 + d2 * c0;
493 for (i = 0; i < nCol; i++) {
494 d1 = V[nCol * i + j];
495 d2 = V[nCol * i + k];
496 V[nCol * i + j] = d1 * c0 + d2 * s0;
497 V[nCol * i + k] = -d1 * s0 + d2 * c0;
502 while (EstColRank >= 3 && S[(EstColRank - 1)] <= S[0] * tol + tol * tol)
505 for(i = 0; i < nCol; i++)
507 for(i = 0; i < nCol; i++)
508 for(j = 0; j < nRow; j++)
509 U[nCol * j + i] = U[nCol * j + i] / S[i];
545 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
546 # include <opencv2/core/core.hpp>
548 int rows = (int)this->
getRows();
549 int cols = (int)this->
getCols();
550 cv::Mat m(rows, cols, CV_64F, this->
data);
551 cv::SVD opencvSVD(m);
552 cv::Mat opencvV = opencvSVD.vt;
553 cv::Mat opencvW = opencvSVD.w;
554 v.
resize((
unsigned int)opencvV.rows, (
unsigned int)opencvV.cols);
555 w.
resize((
unsigned int)(opencvW.rows*opencvW.cols));
557 memcpy(v.
data, opencvV.data, (
size_t)(8*opencvV.rows*opencvV.cols));
559 memcpy(w.
data, opencvW.data, (
size_t)(8*opencvW.rows*opencvW.cols));
560 this->
resize((
unsigned int)opencvSVD.u.rows, (
unsigned int)opencvSVD.u.cols);
561 memcpy(this->
data,opencvSVD.u.data, (
size_t)(8*opencvSVD.u.rows*opencvSVD.u.cols));
566 #ifdef VISP_HAVE_LAPACK
567 extern "C" int dgesdd_(
char *jobz,
int *m,
int *n,
double *a,
int *lda,
double *s,
double *u,
int *ldu,
double *vt,
int *ldvt,
double *work,
int *lwork,
int *iwork,
int *info);
572 int m =
static_cast<int>(this->
getCols()), n = static_cast<int>(this->
getRows()), lda = m, ldu = m, ldvt = std::min(m,n);
578 int* iwork =
new int[8*
static_cast<unsigned int>(std::min(n,m))];
581 double* a =
new double[
static_cast<unsigned int>(lda*n)];
584 double* vt = this->
data;
589 dgesdd_( (
char*)
"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info );
591 work =
new double[
static_cast<unsigned int>(lwork)];
593 dgesdd_( (
char*)
"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, iwork, &info );
596 std::cout <<
"The algorithm computing SVD failed to converge." << std::endl;
608 #include <gsl/gsl_linalg.h>
620 gsl_matrix *A = gsl_matrix_alloc(nr, nc) ;
623 for (i=0 ; i < nr ; i++)
626 for (j=0 ; j < nc ; j++)
627 A->data[k+j] = (*
this)[i][j] ;
631 gsl_matrix *V = gsl_matrix_alloc(nc, nc) ;
632 gsl_vector *S = gsl_vector_alloc(nc) ;
633 gsl_vector *work = gsl_vector_alloc(nc) ;
635 gsl_linalg_SV_decomp(A,V,S, work) ;
643 for (i=0 ; i < nr ; i++)
644 for (j=0 ; j < nc ; j++)
645 (*
this)[i][j] = gsl_matrix_get(A,i,j) ;
648 for (i=0 ; i < nc ; i++)
651 for (j=0 ; j < nc ; j++)
652 v[i][j] = V->
data[k+j] ;
655 for (j=0 ; j < nc ; j++)
656 w[j] = gsl_vector_get(S,j) ;
662 gsl_vector_free(work) ;
664 #else //optimisation Anthony 20/03/2008
668 gsl_vector *work = gsl_vector_alloc(nc) ;
699 gsl_linalg_SV_decomp(&A,&V,&S, work) ;
701 gsl_vector_free(work) ;
713 #endif // doxygen should skip this