00001 #include "matrix_wrapper.h"
00002 #include <math.h>
00003 #include <vector>
00004
00005 using namespace std;
00006
00007 namespace MatrixWrapper
00008 {
00009
00010 bool
00011 SymmetricMatrix_Wrapper::cholesky_semidefinite(MyMatrix& a) const
00012 {
00013
00014
00015
00016 a = (*(MySymmetricMatrix*)this);
00017 int sz = a.rows();
00018 for (int k=1; k<sz+1; ++k) {
00019
00020 if (a(k,k)< std::numeric_limits<double>::epsilon() && a(k,k)> -std::numeric_limits<double>::epsilon()){
00021 a(k,k) = 0.0;
00022 }
00023 if (a(k,k) < 0.0) {
00024 std::cout<< "Warning: matrix of which cholesky decomposition is asked, is negative definite!: returning zero matrix" << std::endl;
00025 std::cout<< "a(" << k << "," << k << ")=" << a(k,k) << std::endl;
00026 a = 0.0; return false;
00027 }
00028 else a(k,k)=sqrt(a(k,k));
00029 for (int i=k+1; i<sz+1; ++i) {
00030 if (a(k,k)< std::numeric_limits<double>::epsilon()){
00031 a(i,k) = 0.0;
00032 }
00033 else a(i,k)/=a(k,k);
00034 }
00035 for (int j=k+1; j<sz+1; ++j) {
00036 for (int i=j; i<sz+1; ++i) a(i,j)-=a(i,k)*a(j,k);
00037 }
00038 }
00039
00040 for (int i=1; i<sz+1; i++) {
00041 for (int j=i+1; j<sz+1; j++) a(i,j)=0.0;
00042 }
00043 return true;
00044 }
00045
00046 bool
00047 Matrix_Wrapper::SVD(MyColumnVector& w, MyMatrix& U, MyMatrix& V) const
00048 {
00049
00050 const int rows=this->rows();
00051
00052 const int cols=this->columns();
00053
00054 U = *((MyMatrix*)(this));
00055
00056 static const int maxIter=150;
00057
00058 w.resize(cols);
00059 V.resize(cols,cols,false,true);
00060 int i(-1),its(-1),j(-1),jj(-1),k(-1),nm(0);
00061 int ppi(0);
00062 bool flag;
00063 double maxarg1, maxarg2;
00064 double anorm(0),c(0),f(0),g(0),h(0),s(0),scale(0),x(0),y(0),z(0);
00065
00066
00067 std::vector<double> rv1(cols,0.0);
00068
00069 g=scale=anorm=0.0;
00070
00071 for (i=1; i <= cols; i++){
00072 ppi=i+1;
00073 rv1.at(i-1)=scale*g;
00074 g=s=scale=0.0;
00075 if (i <= rows ) {
00076
00077 for(k=i;k<=rows;k++) scale += fabs(U(k,i));
00078 if (scale) {
00079
00080
00081 for (k=i;k<=rows;k++){
00082 U(k,i)/= scale;
00083 s += U(k,i) * U(k,i);
00084 }
00085 f=U(i,i);
00086 g=-SIGN(sqrt(s),f);
00087 h=f*g-s;
00088 U(i,i)=f-g;
00089 for (j=ppi; j <= cols; j++) {
00090
00091 for (s=0.0,k=i;k<=rows;k++) s += U(k,i) * U(k,j);
00092 f=s/h;
00093
00094 for (k=i;k<=rows;k++) U(k,j) += f*U(k,i);
00095 }
00096 for (k=i;k<=rows;k++) U(k,i) *= scale;
00097 }
00098 }
00099
00100 w(i) = scale*g;
00101 g=s=scale=0.0;
00102 if ((i <= rows) && (i != cols)) {
00103
00104 for(k=ppi;k<=cols;k++) scale += fabs(U(i,k));
00105 if (scale) {
00106 for(k=ppi;k<=cols;k++){
00107 U(i,k) /= scale;
00108 s += U(i,k)*U(i,k);
00109 }
00110 f=U(i,ppi);
00111 g=-SIGN(sqrt(s),f);
00112 h=f*g-s;
00113 U(i,ppi)=f-g;
00114 for ( k=ppi; k <= cols; k++) rv1.at(k-1)=U(i,k)/h;
00115 for ( j=ppi; j <= rows; j++) {
00116 for( s=0.0,k=ppi;k<=cols;k++) s += U(j,k) * U(i,k);
00117 for ( k=ppi; k <= cols; k++) U(j,k) += s*rv1.at(k-1);
00118 }
00119 for( k=ppi;k<=cols;k++) U(i,k) *= scale;
00120 }
00121 }
00122 maxarg1=anorm;
00123 maxarg2=(fabs(w(i))+fabs(rv1.at(i-1)));
00124 anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
00125
00126 }
00127
00128
00129 for (i= cols ; i>=1; i--) {
00130 if ( i < cols ) {
00131 if (g) {
00132 for ( j=ppi; j <= cols; j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
00133 for ( j=ppi; j <= cols; j++) {
00134 for (s=0.0,k=ppi;k <= cols;k++) s += U(i,k)*V(k,j);
00135 for ( k=ppi; k <= cols;k++) V(k,j) += s*V(k,i);
00136 }
00137 }
00138 for( j=ppi; j<=cols ; j++) V(i,j)=V(j,i)=0.0;
00139 }
00140 V(i,i)=1.0;
00141 g=rv1.at(i-1);
00142 ppi=i;
00143 }
00144
00145
00146 for (i= cols < rows ? cols: rows; i>=1; i--) {
00147 ppi=i+1;
00148 g=w(i);
00149 for( j=ppi; j<=cols ; j++) U(i,j)=0.0;
00150 if (g) {
00151 g=1.0/g;
00152 for ( j=ppi; j <= cols; j++) {
00153 for( s=0.0, k=ppi; k<=rows ; k++) s += U(k,i)*U(k,j);
00154 f=(s/U(i,i))*g;
00155 for ( k=i; k <= rows;k++) U(k,j) += f*U(k,i);
00156 }
00157 for( j=i; j<=rows ; j++) U(j,i) *=g;
00158 } else {
00159 for( j=i; j<=rows ; j++) U(j,i) = 0.0;
00160 }
00161 ++U(i,i);
00162 }
00163
00164
00165
00166 for ( k=cols; k >= 1; k--) {
00167 for (its=1; its <= maxIter; its++) {
00168 flag=true;
00169
00170 for (ppi=k; ppi >= 1; ppi--) {
00171 nm=ppi-1;
00172 if ((fabs(rv1.at(ppi-1))+anorm) == anorm) {
00173 flag=false;
00174 break;
00175 }
00176 if ((fabs(w(nm)+anorm) == anorm)) {
00177 break;
00178 }
00179 }
00180
00181 if (flag) {
00182 c=0.0;
00183 s=1.0;
00184 for ( i=ppi; i <= k ;i++) {
00185 f=s*rv1.at(i-1);
00186 rv1.at(i-1)=c*rv1.at(i-1);
00187 if ((fabs(f)+anorm) == anorm) {
00188 break;
00189 }
00190 g=w(i);
00191 h=PYTHAG(f,g);
00192 w(i)=h;
00193 h=1.0/h;
00194 c=g*h;
00195 s=-f*h;
00196 for ( j=1;j <= rows; j++) {
00197 y=U(j,nm);
00198 z=U(j,i);
00199 U(j,nm)=y*c+z*s;
00200 U(j,i)=z*c-y*s;
00201 }
00202 }
00203 }
00204 z=w(k);
00205
00206
00207 if (ppi == k) {
00208 if (z < 0.0) {
00209 w(k)=-z;
00210 for (j=1; j <= cols; j++) V(j,k)=-V(j,k);
00211 }
00212 break;
00213 }
00214
00215 if (its == maxIter) {
00216
00217 std::cout << "SVD did not converge after " << maxIter << " iterations!" << std::endl;
00218
00219 w = 0.0;
00220 return false;
00221 }
00222
00223 x=w(ppi);
00224 nm=k-1;
00225 y=w(nm);
00226 g=rv1.at(nm-1);
00227 h=rv1.at(k-1);
00228 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00229
00230 g = PYTHAG(f,1.0);
00231 f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00232
00233
00234 c=s=1.0;
00235 for ( j=ppi; j<=nm ;j++){
00236 i=j+1;
00237 g=rv1.at(i-1);
00238 y=w(i);
00239 h=s*g;
00240 g=c*g;
00241 z=PYTHAG(f,h);
00242 rv1.at(j-1)=z;
00243 c=f/z;
00244 s=h/z;
00245 f=x*c+g*s;
00246 g=g*c-x*s;
00247 h=y*s;
00248 y*=c;
00249 for (jj=1; jj<=cols ;jj++){
00250 x=V(jj,j);
00251 z=V(jj,i);
00252 V(jj,j)=x*c+z*s;
00253 V(jj,i)=z*c-x*s;
00254 }
00255 z=PYTHAG(f,h);
00256 w(j)=z;
00257
00258 if (z) {
00259 z=1.0/z;
00260 c=f*z;
00261 s=h*z;
00262 }
00263 f=c*g+s*y;
00264 x=c*y-s*g;
00265 for (jj=1; jj<=rows; jj++){
00266 y=U(jj,j);
00267 z=U(jj,i);
00268 U(jj,j)=y*c+z*s;
00269 U(jj,i)=z*c-y*s;
00270 }
00271 }
00272 rv1.at(ppi-1)=0.0;
00273 rv1.at(k-1)=f;
00274 w(k)=x;
00275
00276 }
00277 }
00278 return true;
00279 }
00280
00281
00282 double
00283 Matrix_Wrapper::PYTHAG(double a,double b) const
00284 {
00285 double at,bt,ct;
00286 at = fabs(a);
00287 bt = fabs(b);
00288 if (at > bt ) {
00289 ct=bt/at;
00290 return at*sqrt(1.0+ct*ct);
00291 } else {
00292 if (bt==0)
00293 return 0.0;
00294 else {
00295 ct=at/bt;
00296 return bt*sqrt(1.0+ct*ct);
00297 }
00298 }
00299 }
00300
00301
00302 double
00303 Matrix_Wrapper::SIGN(double a,double b) const
00304 {
00305 return ((b) >= 0.0 ? fabs(a) : -fabs(a));
00306 }
00307
00308
00309 MyMatrix
00310 Matrix_Wrapper::pseudoinverse(double epsilon) const
00311 {
00312 int rows;
00313 rows = this->rows();
00314 int cols = this->columns();
00315
00316 MyMatrix U,V;
00317 MyColumnVector D;
00318
00319 bool res;
00320 res = SVD(D,U,V);
00321 assert(res);
00322
00323 Matrix Dinv(cols,cols);
00324 Dinv = 0;
00325 for (unsigned int i=0; i<D.rows(); i++)
00326 if ( D(i+1) < epsilon )
00327 Dinv(i+1,i+1) = 0;
00328 else
00329 Dinv(i+1,i+1) = 1/D(i+1);
00330
00331 #ifdef __DEBUG__
00332 std::cout << "MATRIX::pseudoinverse() Dinv =\n" << Dinv << std::endl;
00333 #endif //__DEBUG__
00334
00335 return V * Dinv * U.transpose();
00336 }
00337
00338 }