sandey/blender
1
0
Fork 0
forked from bartvdbraak/blender
Code Pull Requests Activity
aa42934cef
blender/intern/itasc/kdl/utilities/svd_eigen_HH.hpp

310 lines
10 KiB
C++
Raw Normal View History

Merge of itasc branch. Project files, scons and cmake should be working. Makefile updated but not tested. Comes with Eigen2 2.0.6 C++ matrix library.
2009-09-24 21:22:24 +00:00
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
#ifndef SVD_EIGEN_HH_HPP
#define SVD_EIGEN_HH_HPP
remove $Id: tags after discussion on the mailign list: http://markmail.org/message/fp7ozcywxum3ar7n
2011-10-23 17:52:20 +00:00
#include <Eigen/Core>
Merge of itasc branch. Project files, scons and cmake should be working. Makefile updated but not tested. Comes with Eigen2 2.0.6 C++ matrix library.
2009-09-24 21:22:24 +00:00
#include <algorithm>
namespace KDL
{
template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
double at,bt,ct;
at = fabs(a);
bt = fabs(b);
if (at > bt ) {
ct=bt/at;
return Scalar(at*sqrt(1.0+ct*ct));
} else {
if (bt==0)
return Scalar(0.0);
else {
ct=at/bt;
return Scalar(bt*sqrt(1.0+ct*ct));
}
}
}
template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
}
/**
* svd calculation of boost ublas matrices
*
* @param A matrix<double>(mxn)
* @param U matrix<double>(mxn)
* @param S vector<double> n
* @param V matrix<double>(nxn)
* @param tmp vector<double> n
* @param maxiter defaults to 150
*
* @return -2 if maxiter exceeded, 0 otherwise
*/
template<typename MatrixA, typename MatrixUV, typename VectorS>
int svd_eigen_HH(
const Eigen::MatrixBase<MatrixA>& A,
Eigen::MatrixBase<MatrixUV>& U,
Eigen::MatrixBase<VectorS>& S,
Eigen::MatrixBase<MatrixUV>& V,
Eigen::MatrixBase<VectorS>& tmp,
int maxiter=150)
{
//get the rows/columns of the matrix
const int rows = A.rows();
const int cols = A.cols();
U = A;
int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
int ppi(0);
bool flag;
e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
g=scale=anorm=e_scalar(0.0);
/* Householder reduction to bidiagonal form. */
for (i=0;i<cols;i++) {
ppi=i+1;
tmp(i)=scale*g;
g=s=scale=e_scalar(0.0);
if (i<rows) {
// compute the sum of the i-th column, starting from the i-th row
for (k=i;k<rows;k++) scale += fabs(U(k,i));
if (scale!=0) {
// multiply the i-th column by 1.0/scale, start from the i-th element
// sum of squares of column i, start from the i-th element
for (k=i;k<rows;k++) {
U(k,i) /= scale;
s += U(k,i)*U(k,i);
}
f=U(i,i); // f is the diag elem
g = -SIGN(e_scalar(sqrt(s)),f);
h=f*g-s;
U(i,i)=f-g;
for (j=ppi;j<cols;j++) {
// dot product of columns i and j, starting from the i-th row
for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
f=s/h;
// copy the scaled i-th column into the j-th column
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
}
for (k=i;k<rows;k++) U(k,i) *= scale;
}
}
// save singular value
S(i)=scale*g;
g=s=scale=e_scalar(0.0);
if ((i <rows) && (i+1 != cols)) {
// sum of row i, start from columns i+1
for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
if (scale!=0) {
for (k=ppi;k<cols;k++) {
U(i,k) /= scale;
s += U(i,k)*U(i,k);
}
f=U(i,ppi);
g = -SIGN(e_scalar(sqrt(s)),f);
h=f*g-s;
U(i,ppi)=f-g;
for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
for (j=ppi;j<rows;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
}
for (k=ppi;k<cols;k++) U(i,k) *= scale;
}
}
maxarg1=anorm;
maxarg2=(fabs(S(i))+fabs(tmp(i)));
anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
}
/* Accumulation of right-hand transformations. */
for (i=cols-1;i>=0;i--) {
if (i<cols-1) {
if (g) {
for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
}
}
for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
}
V(i,i)=1.0;
g=tmp(i);
ppi=i;
}
/* Accumulation of left-hand transformations. */
for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
ppi=i+1;
g=S(i);
for (j=ppi;j<cols;j++) U(i,j)=0.0;
if (g) {
g=e_scalar(1.0)/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
f=(s/U(i,i))*g;
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
}
for (j=i;j<rows;j++) U(j,i) *= g;
} else {
for (j=i;j<rows;j++) U(j,i)=0.0;
}
++U(i,i);
}
/* Diagonalization of the bidiagonal form. */
for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */
flag=true;
for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */
nm=ppi-1; /* Note that tmp(1) is always zero. */
if ((fabs(tmp(ppi))+anorm) == anorm) {
flag=false;
break;
}
if ((fabs(S(nm)+anorm) == anorm)) break;
}
if (flag) {
c=e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */
s=e_scalar(1.);
for (i=ppi;i<=k;i++) {
f=s*tmp(i);
tmp(i)=c*tmp(i);
if ((fabs(f)+anorm) == anorm) break;
g=S(i);
h=PYTHAG(f,g);
S(i)=h;
h=e_scalar(1.0)/h;
c=g*h;
s=(-f*h);
for (j=0;j<rows;j++) {
y=U(j,nm);
z=U(j,i);
U(j,nm)=y*c+z*s;
U(j,i)=z*c-y*s;
}
}
}
z=S(k);
if (ppi == k) { /* Convergence. */
if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */
S(k) = -z;
for (j=0;j<cols;j++) V(j,k)=-V(j,k);
}
break;
}
x=S(ppi); /* Shift from bottom 2-by-2 minor: */
nm=k-1;
y=S(nm);
g=tmp(nm);
h=tmp(k);
f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);
g=PYTHAG(f,e_scalar(1.0));
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
/* Next QR transformation: */
c=s=1.0;
for (j=ppi;j<=nm;j++) {
i=j+1;
g=tmp(i);
y=S(i);
h=s*g;
g=c*g;
z=PYTHAG(f,h);
tmp(j)=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y=y*c;
for (jj=0;jj<cols;jj++) {
x=V(jj,j);
z=V(jj,i);
V(jj,j)=x*c+z*s;
V(jj,i)=z*c-x*s;
}
z=PYTHAG(f,h);
S(j)=z;
if (z) {
z=e_scalar(1.0)/z;
c=f*z;
s=h*z;
}
f=(c*g)+(s*y);
x=(c*y)-(s*g);
for (jj=0;jj<rows;jj++) {
y=U(jj,j);
z=U(jj,i);
U(jj,j)=y*c+z*s;
U(jj,i)=z*c-y*s;
}
}
tmp(ppi)=0.0;
tmp(k)=f;
S(k)=x;
}
}
//Sort eigen values:
for (i=0; i<cols; i++){
double S_max = S(i);
int i_max = i;
for (j=i+1; j<cols; j++){
double Sj = S(j);
if (Sj > S_max){
S_max = Sj;
i_max = j;
}
}
if (i_max != i){
/* swap eigenvalues */
e_scalar tmp = S(i);
S(i)=S(i_max);
S(i_max)=tmp;
/* swap eigenvectors */
U.col(i).swap(U.col(i_max));
V.col(i).swap(V.col(i_max));
}
}
if (its == maxiter)
return (-2);
else
return (0);
}
}
#endif
Copy Permalink