Added SingularValueDecomposition method
This commit is contained in:
parent
1002d536ff
commit
7767e4a63b
|
|
@ -13,7 +13,7 @@ namespace vcg
|
|||
* \param nrot returns the number of Jacobi rotations that were required.
|
||||
*/
|
||||
template <typename TYPE>
|
||||
static void Jacobi(Matrix44<TYPE> &w, Point4<TYPE> &d, Matrix44<TYPE> &v, int &nrot)
|
||||
static void Jacobi(Matrix44<TYPE> &w, Point4<TYPE> &d, Matrix44<TYPE> &v, int &nrot)
|
||||
{
|
||||
int j,iq,ip,i;
|
||||
//assert(w.IsSymmetric());
|
||||
|
|
@ -112,15 +112,259 @@ namespace vcg
|
|||
/*!
|
||||
* Given a matrix <I>A<SUB>m×n</SUB></I>, this routine computes its singular value decomposition,
|
||||
* i.e. <I>A=U·W·V<SUP>T</SUP></I>. The matrix <I>A</I> will be destroyed!
|
||||
* \param A ...
|
||||
* (This is the implementation described in <I>Numerical Recipies</I>).
|
||||
* \param A the matrix to be decomposed
|
||||
* \param W the diagonal matrix of singular values <I>W</I>, stored as a vector <I>W[1...N]</I>
|
||||
* \param V the matrix <I>V</I> (not the transpose <I>V<SUP>T</SUP></I>)
|
||||
* \param max_iters max iteration number (default = 30).
|
||||
* \return
|
||||
*/
|
||||
template <typename MATRIX_TYPE>
|
||||
static void SingularValueDecomposition(MATRIX_TYPE &A, typename MATRIX_TYPE::ScalarType *W, MATRIX_TYPE &V)
|
||||
static bool SingularValueDecomposition(MATRIX_TYPE &A, typename MATRIX_TYPE::ScalarType *W, MATRIX_TYPE &V, const int max_iters = 30)
|
||||
{
|
||||
typedef typename MATRIX_TYPE::ScalarType ScalarType;
|
||||
int m = (int) A.RowsNumber();
|
||||
int n = (int) A.ColumnsNumber();
|
||||
int flag,i,its,j,jj,k,l,nm;
|
||||
double anorm, c, f, g, h, s, scale, x, y, z, *rv1;
|
||||
bool convergence = true;
|
||||
|
||||
rv1 = new double[n];
|
||||
g = scale = anorm = 0;
|
||||
// Householder reduction to bidiagonal form.
|
||||
for (i=0; i<n; i++)
|
||||
{
|
||||
l = i+1;
|
||||
rv1[i] = scale*g;
|
||||
g = s = scale = 0.0;
|
||||
if (i < m)
|
||||
{
|
||||
for (k = i; k<m; k++)
|
||||
scale += fabs(A[k][i]);
|
||||
if (scale)
|
||||
{
|
||||
for (k=i; k<m; k++)
|
||||
{
|
||||
A[k][i] /= scale;
|
||||
s += A[k][i]*A[k][i];
|
||||
}
|
||||
f=A[i][i];
|
||||
g = -vcg::sign<double>( sqrt(s), f );
|
||||
h = f*g - s;
|
||||
A[i][i]=f-g;
|
||||
for (j=l; j<n; j++)
|
||||
{
|
||||
for (s=0.0, k=i; k<m; k++)
|
||||
s += A[k][i]*A[k][j];
|
||||
f = s/h;
|
||||
for (k=i; k<m; k++)
|
||||
A[k][j] += f*A[k][i];
|
||||
}
|
||||
for (k=i; k<m; k++)
|
||||
A[k][i] *= scale;
|
||||
}
|
||||
}
|
||||
W[i] = scale *g;
|
||||
g = s = scale = 0.0;
|
||||
if (i < m && i != (n-1))
|
||||
{
|
||||
for (k=l; k<n; k++)
|
||||
scale += fabs(A[i][k]);
|
||||
if (scale)
|
||||
{
|
||||
for (k=l; k<n; k++)
|
||||
{
|
||||
A[i][k] /= scale;
|
||||
s += A[i][k]*A[i][k];
|
||||
}
|
||||
f = A[i][l];
|
||||
g = -vcg::sign<double>(sqrt(s),f);
|
||||
h = f*g - s;
|
||||
A[i][l] = f-g;
|
||||
for (k=l; k<n; k++)
|
||||
rv1[k] = A[i][k]/h;
|
||||
for (j=l; j<m; j++)
|
||||
{
|
||||
for (s=0.0, k=l; k<n; k++)
|
||||
s += A[j][k]*A[i][k];
|
||||
for (k=l; k<n; k++)
|
||||
A[j][k] += s*rv1[k];
|
||||
}
|
||||
for (k=l; k<n; k++)
|
||||
A[i][k] *= scale;
|
||||
}
|
||||
}
|
||||
anorm=math::Max( anorm, (fabs(W[i])+fabs(rv1[i])) );
|
||||
}
|
||||
// Accumulation of right-hand transformations.
|
||||
for (i=(n-1); i>=0; i--)
|
||||
{
|
||||
//Accumulation of right-hand transformations.
|
||||
if (i < (n-1))
|
||||
{
|
||||
if (g)
|
||||
{
|
||||
for (j=l; j<n;j++) //Double division to avoid possible underflow.
|
||||
V[j][i]=(A[i][j]/A[i][l])/g;
|
||||
for (j=l; j<n; j++)
|
||||
{
|
||||
for (s=0.0, k=l; k<n; k++)
|
||||
s += A[i][k] * V[k][j];
|
||||
for (k=l; k<n; k++)
|
||||
V[k][j] += s*V[k][i];
|
||||
}
|
||||
}
|
||||
for (j=l; j<n; j++)
|
||||
V[i][j] = V[j][i] = 0.0;
|
||||
}
|
||||
V[i][i] = 1.0;
|
||||
g = rv1[i];
|
||||
l = i;
|
||||
}
|
||||
// Accumulation of left-hand transformations.
|
||||
for (i=math::Min(m,n)-1; i>=0; i--)
|
||||
{
|
||||
l = i+1;
|
||||
g = W[i];
|
||||
for (j=l; j<n; j++)
|
||||
A[i][j]=0.0;
|
||||
if (g)
|
||||
{
|
||||
g = 1.0/g;
|
||||
for (j=l; j<n; j++)
|
||||
{
|
||||
for (s=0.0, k=l; k<m; k++)
|
||||
s += A[k][i]*A[k][j];
|
||||
f = (s/A[i][i])*g;
|
||||
for (k=i; k<m; k++)
|
||||
A[k][j] += f*A[k][i];
|
||||
}
|
||||
for (j=i; j<m; j++)
|
||||
A[j][i] *= g;
|
||||
}
|
||||
else
|
||||
for (j=i; j<m; j++)
|
||||
A[j][i] = 0.0;
|
||||
++A[i][i];
|
||||
}
|
||||
// Diagonalization of the bidiagonal form: Loop over
|
||||
// singular values, and over allowed iterations.
|
||||
for (k=(n-1); k>=0; k--)
|
||||
{
|
||||
for (its=1; its<=max_iters; its++)
|
||||
{
|
||||
flag=1;
|
||||
for (l=k; l>=0; l--)
|
||||
{
|
||||
// Test for splitting.
|
||||
nm=l-1;
|
||||
// Note that rv1[1] is always zero.
|
||||
if ((double)(fabs(rv1[l])+anorm) == anorm)
|
||||
{
|
||||
flag=0;
|
||||
break;
|
||||
}
|
||||
if ((double)(fabs(W[nm])+anorm) == anorm)
|
||||
break;
|
||||
}
|
||||
if (flag)
|
||||
{
|
||||
c=0.0; //Cancellation of rv1[l], if l > 1.
|
||||
s=1.0;
|
||||
for (i=l ;i<=k; i++)
|
||||
{
|
||||
f = s*rv1[i];
|
||||
rv1[i] = c*rv1[i];
|
||||
if ((double)(fabs(f)+anorm) == anorm)
|
||||
break;
|
||||
g = W[i];
|
||||
h = pythagora< double >(f,g);
|
||||
W[i] = h;
|
||||
h = 1.0/h;
|
||||
c = g*h;
|
||||
s = -f*h;
|
||||
for (j=0; j<m; j++)
|
||||
{
|
||||
y = A[j][nm];
|
||||
z = A[j][i];
|
||||
A[j][nm] = y*c + z*s;
|
||||
A[j][i] = z*c - y*s;
|
||||
}
|
||||
}
|
||||
}
|
||||
z = W[k];
|
||||
if (l == k) //Convergence.
|
||||
{
|
||||
if (z < 0.0) { // Singular value is made nonnegative.
|
||||
W[k] = -z;
|
||||
for (j=0; j<n; j++)
|
||||
V[j][k] = -V[j][k];
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (its == max_iters)
|
||||
{
|
||||
printf("no convergence in %d SingularValueDecomposition iterations\n", max_iters);
|
||||
convergence = false;
|
||||
}
|
||||
x = W[l]; // Shift from bottom 2-by-2 minor.
|
||||
nm = k-1;
|
||||
y = W[nm];
|
||||
g = rv1[nm];
|
||||
h = rv1[k];
|
||||
f = ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
|
||||
g = pythagora<double>(f,1.0);
|
||||
f=((x-z)*(x+z) + h*((y/(f+sign(g,f)))-h))/x;
|
||||
c=s=1.0;
|
||||
//Next QR transformation:
|
||||
for (j=l; j<= nm;j++)
|
||||
{
|
||||
i = j+1;
|
||||
g = rv1[i];
|
||||
y = W[i];
|
||||
h = s*g;
|
||||
g = c*g;
|
||||
z = pythagora<double>(f,h);
|
||||
rv1[j] = z;
|
||||
c = f/z;
|
||||
s = h/z;
|
||||
f = x*c + g*s;
|
||||
g = g*c - x*s;
|
||||
h = y*s;
|
||||
y *= c;
|
||||
for (jj=0; jj<n; jj++)
|
||||
{
|
||||
x = V[jj][j];
|
||||
z = V[jj][i];
|
||||
V[jj][j] = x*c + z*s;
|
||||
V[jj][i] = z*c - x*s;
|
||||
}
|
||||
z = pythagora<double>(f,h);
|
||||
W[j] = z;
|
||||
// Rotation can be arbitrary if z = 0.
|
||||
if (z)
|
||||
{
|
||||
z = 1.0/z;
|
||||
c = f*z;
|
||||
s = h*z;
|
||||
}
|
||||
f = c*g + s*y;
|
||||
x = c*y - s*g;
|
||||
for (jj=0; jj<m; jj++)
|
||||
{
|
||||
y = A[jj][j];
|
||||
z = A[jj][i];
|
||||
A[jj][j] = y*c + z*s;
|
||||
A[jj][i] = z*c - y*s;
|
||||
}
|
||||
}
|
||||
rv1[l] = 0.0;
|
||||
rv1[k] = f;
|
||||
W[k] = x;
|
||||
}
|
||||
}
|
||||
delete []rv1;
|
||||
return convergence;
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -174,5 +418,18 @@ namespace vcg
|
|||
return (abs_b == 0.0 ? 0.0 : abs_b*sqrt(1.0+sqr(abs_a/abs_b)));
|
||||
};
|
||||
|
||||
template <typename TYPE>
|
||||
inline TYPE sign(TYPE a, TYPE b)
|
||||
{
|
||||
return (b >= 0.0 ? fabs(a) : -fabs(a));
|
||||
};
|
||||
|
||||
template <typename TYPE>
|
||||
inline TYPE sqr(TYPE a)
|
||||
{
|
||||
TYPE sqr_arg = a;
|
||||
return (sqr_arg == 0 ? 0 : sqr_arg*sqr_arg);
|
||||
}
|
||||
|
||||
/*! @} */
|
||||
}; // end of namespace
|
||||
Loading…
Reference in New Issue