00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "svd_eigen_HH.hpp"
00023
00024 namespace KDL{
00025
00026 int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter,double epsilon)
00027 {
00028
00029 const int rows = A.rows();
00030 const int cols = A.cols();
00031
00032 U.setZero();
00033 U.topLeftCorner(rows,cols)=A;
00034
00035 int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
00036 int ppi(0);
00037 bool flag,maxarg1,maxarg2;
00038 double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
00039
00040
00041 for (i=0;i<cols;i++) {
00042 ppi=i+1;
00043 tmp(i)=scale*g;
00044 g=s=scale=0.0;
00045 if (i<rows) {
00046
00047 for (k=i;k<rows;k++) scale += fabs(U(k,i));
00048 if (fabs(scale)>epsilon) {
00049
00050
00051 for (k=i;k<rows;k++) {
00052 U(k,i) /= scale;
00053 s += U(k,i)*U(k,i);
00054 }
00055 f=U(i,i);
00056 if (!(s>=0)) return -3;
00057 g = -SIGN(sqrt(s),f);
00058 h=f*g-s;
00059 U(i,i)=f-g;
00060 for (j=ppi;j<cols;j++) {
00061
00062 for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
00063 if (!(h!=0)) return -4;
00064 f=s/h;
00065
00066 for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
00067 }
00068 for (k=i;k<rows;k++) U(k,i) *= scale;
00069 }
00070 }
00071
00072 S(i)=scale*g;
00073 g=s=scale=0.0;
00074 if ((i <rows) && (i+1 != cols)) {
00075
00076 for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
00077 if (fabs(scale)>epsilon) {
00078 for (k=ppi;k<cols;k++) {
00079 U(i,k) /= scale;
00080 s += U(i,k)*U(i,k);
00081 }
00082 f=U(i,ppi);
00083 if (!(s>=0)) return -5;
00084 g = -SIGN(sqrt(s),f);
00085 h=f*g-s;
00086 U(i,ppi)=f-g;
00087 if (!(h!=0)) return -6;
00088 for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
00089 for (j=ppi;j<rows;j++) {
00090 for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
00091 for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
00092 }
00093 for (k=ppi;k<cols;k++) U(i,k) *= scale;
00094 }
00095 }
00096 maxarg1=anorm;
00097 maxarg2=(fabs(S(i))+fabs(tmp(i)));
00098 anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
00099 }
00100
00101 for (i=cols-1;i>=0;i--) {
00102 if (i<cols-1) {
00103 if (fabs(g)>epsilon) {
00104 if (!(U(i,ppi)!=0)) return -7;
00105 for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
00106 for (j=ppi;j<cols;j++) {
00107 for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
00108 for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
00109 }
00110 }
00111 for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
00112 }
00113 V(i,i)=1.0;
00114 g=tmp(i);
00115 ppi=i;
00116 }
00117
00118 for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
00119 ppi=i+1;
00120 g=S(i);
00121 for (j=ppi;j<cols;j++) U(i,j)=0.0;
00122 if (fabs(g)>epsilon) {
00123 g=1.0/g;
00124 for (j=ppi;j<cols;j++) {
00125 for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
00126 if (!(U(i,i)!=0)) return -8;
00127 f=(s/U(i,i))*g;
00128 for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
00129 }
00130 for (j=i;j<rows;j++) U(j,i) *= g;
00131 } else {
00132 for (j=i;j<rows;j++) U(j,i)=0.0;
00133 }
00134 ++U(i,i);
00135 }
00136
00137
00138 for (k=cols-1;k>=0;k--) {
00139 for (its=1;its<=maxiter;its++) {
00140 flag=true;
00141 for (ppi=k;ppi>=0;ppi--) {
00142 nm=ppi-1;
00143 if ((fabs(tmp(ppi))+anorm) == anorm) {
00144 flag=false;
00145 break;
00146 }
00147 if ((fabs(S(nm)+anorm) == anorm)) break;
00148 }
00149 if (flag) {
00150 c=0.0;
00151 s=1.0;
00152 for (i=ppi;i<=k;i++) {
00153 f=s*tmp(i);
00154 tmp(i)=c*tmp(i);
00155 if ((fabs(f)+anorm) == anorm) break;
00156 g=S(i);
00157 h=PYTHAG(f,g);
00158 S(i)=h;
00159 if (!(h!=0)) return -9;
00160 h=1.0/h;
00161 c=g*h;
00162 s=(-f*h);
00163 for (j=0;j<rows;j++) {
00164 y=U(j,nm);
00165 z=U(j,i);
00166 U(j,nm)=y*c+z*s;
00167 U(j,i)=z*c-y*s;
00168 }
00169 }
00170 }
00171 z=S(k);
00172
00173 if (ppi == k) {
00174 if (z < 0.0) {
00175 S(k) = -z;
00176 for (j=0;j<cols;j++) V(j,k)=-V(j,k);
00177 }
00178 break;
00179 }
00180
00181 x=S(ppi);
00182 nm=k-1;
00183 y=S(nm);
00184 g=tmp(nm);
00185 h=tmp(k);
00186 if (!(h!=0&&y!=0)) return -10;
00187 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00188
00189 g=PYTHAG(f,1.0);
00190 if (!(x!=0)) return -11;
00191 if (!((f+SIGN(g,f))!=0)) return -12;
00192 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00193
00194
00195 c=s=1.0;
00196 for (j=ppi;j<=nm;j++) {
00197 i=j+1;
00198 g=tmp(i);
00199 y=S(i);
00200 h=s*g;
00201 g=c*g;
00202 z=PYTHAG(f,h);
00203 tmp(j)=z;
00204 if (!(z!=0)) return -13;
00205 c=f/z;
00206 s=h/z;
00207 f=x*c+g*s;
00208 g=g*c-x*s;
00209 h=y*s;
00210 y=y*c;
00211 for (jj=0;jj<cols;jj++) {
00212 x=V(jj,j);
00213 z=V(jj,i);
00214 V(jj,j)=x*c+z*s;
00215 V(jj,i)=z*c-x*s;
00216 }
00217 z=PYTHAG(f,h);
00218 S(j)=z;
00219 if (fabs(z)>epsilon) {
00220 z=1.0/z;
00221 c=f*z;
00222 s=h*z;
00223 }
00224 f=(c*g)+(s*y);
00225 x=(c*y)-(s*g);
00226 for (jj=0;jj<rows;jj++) {
00227 y=U(jj,j);
00228 z=U(jj,i);
00229 U(jj,j)=y*c+z*s;
00230 U(jj,i)=z*c-y*s;
00231 }
00232 }
00233 tmp(ppi)=0.0;
00234 tmp(k)=f;
00235 S(k)=x;
00236 }
00237 }
00238
00239
00240 for (i=0; i<cols; i++){
00241
00242 double S_max = S(i);
00243 int i_max = i;
00244 for (j=i+1; j<cols; j++){
00245 double Sj = S(j);
00246 if (Sj > S_max){
00247 S_max = Sj;
00248 i_max = j;
00249 }
00250 }
00251 if (i_max != i){
00252
00253 double tmp = S(i);
00254 S(i)=S(i_max);
00255 S(i_max)=tmp;
00256
00257
00258 U.col(i).swap(U.col(i_max));
00259 V.col(i).swap(V.col(i_max));
00260 }
00261 }
00262
00263 if (its == maxiter)
00264 return (-2);
00265 else
00266 return (0);
00267 }
00268 }