matrix_wrapper.cpp
Go to the documentation of this file.
1 #include "matrix_wrapper.h"
2 #include <math.h>
3 #include <vector>
4 
5 using namespace std;
6 
7 namespace MatrixWrapper
8 {
9 
10  bool
11  SymmetricMatrix_Wrapper::cholesky_semidefinite(MyMatrix& a) const
12  {
13  // apply cholesky to *this
14  // put result in a
15  // result is lower triangular matrix
16  a = (*(MySymmetricMatrix*)this);
17  int sz = a.rows();
18  for (int k=1; k<sz+1; ++k) {
19  // check if close to zero => put to zero
20  if (a(k,k)< std::numeric_limits<double>::epsilon() && a(k,k)> -std::numeric_limits<double>::epsilon()){
21  a(k,k) = 0.0; //set to zero, matrix is semidefinite
22  }
23  if (a(k,k) < 0.0) {
24  std::cout<< "Warning: matrix of which cholesky decomposition is asked, is negative definite!: returning zero matrix" << std::endl;
25  std::cout<< "a(" << k << "," << k << ")=" << a(k,k) << std::endl;
26  a = 0.0; return false;//matrix is negative definite
27  }
28  else a(k,k)=sqrt(a(k,k));
29  for (int i=k+1; i<sz+1; ++i) {
30  if (a(k,k)< std::numeric_limits<double>::epsilon()){
31  a(i,k) = 0.0; //set to zero, matrix is semidefinite
32  }
33  else a(i,k)/=a(k,k);
34  }
35  for (int j=k+1; j<sz+1; ++j) {
36  for (int i=j; i<sz+1; ++i) a(i,j)-=a(i,k)*a(j,k);
37  }
38  }
39  //delete upper triangle
40  for (int i=1; i<sz+1; i++) {
41  for (int j=i+1; j<sz+1; j++) a(i,j)=0.0;
42  }
43  return true;
44  }
45 
46  bool
47  Matrix_Wrapper::SVD(MyColumnVector& w, MyMatrix& U, MyMatrix& V) const
48  {
49  //get the rows of the matrix
50  const int rows=this->rows();
51  //get the columns of the matrix
52  const int cols=this->columns();
53 
54  U = *((MyMatrix*)(this));
55 
56  static const int maxIter=150;
57 
58  w.resize(cols);
59  V.resize(cols,cols,false,true);
60  int i(-1),its(-1),j(-1),jj(-1),k(-1),nm(0);
61  int ppi(0);
62  bool flag;
63  double maxarg1, maxarg2;
64  double anorm(0),c(0),f(0),g(0),h(0),s(0),scale(0),x(0),y(0),z(0);
65 
66  // Householder reduction to bidiagonal form
67  std::vector<double> rv1(cols,0.0);
68 
69  g=scale=anorm=0.0;
70 
71  for (i=1; i <= cols; i++){
72  ppi=i+1;
73  rv1.at(i-1)=scale*g;
74  g=s=scale=0.0;
75  if (i <= rows ) {
76  // compute the sum of the i-th column, starting from the i-th row
77  for(k=i;k<=rows;k++) scale += fabs(U(k,i));
78  if (scale) {
79  // multiply the i-th column by 1.0/scale, start from the i-th element
80  // sum of squares of column i, start from the i-th element
81  for (k=i;k<=rows;k++){
82  U(k,i)/= scale;
83  s += U(k,i) * U(k,i);
84  }
85  f=U(i,i); // f is the diag elem
86  g=-SIGN(sqrt(s),f);
87  h=f*g-s;
88  U(i,i)=f-g;
89  for (j=ppi; j <= cols; j++) {
90  // dot product of columns i and j, starting from the i-th row
91  for (s=0.0,k=i;k<=rows;k++) s += U(k,i) * U(k,j);
92  f=s/h;
93  // copy the scaled i-th column into the j-th column
94  for (k=i;k<=rows;k++) U(k,j) += f*U(k,i);
95  }
96  for (k=i;k<=rows;k++) U(k,i) *= scale;
97  }
98  }
99  // save singular value
100  w(i) = scale*g;
101  g=s=scale=0.0;
102  if ((i <= rows) && (i != cols)) {
103  // sum of row i, start from columns i+1
104  for(k=ppi;k<=cols;k++) scale += fabs(U(i,k));
105  if (scale) {
106  for(k=ppi;k<=cols;k++){
107  U(i,k) /= scale;
108  s += U(i,k)*U(i,k);
109  }
110  f=U(i,ppi);
111  g=-SIGN(sqrt(s),f); //<---- do something
112  h=f*g-s;
113  U(i,ppi)=f-g;
114  for ( k=ppi; k <= cols; k++) rv1.at(k-1)=U(i,k)/h;
115  for ( j=ppi; j <= rows; j++) {
116  for( s=0.0,k=ppi;k<=cols;k++) s += U(j,k) * U(i,k);
117  for ( k=ppi; k <= cols; k++) U(j,k) += s*rv1.at(k-1);
118  }
119  for( k=ppi;k<=cols;k++) U(i,k) *= scale;
120  }
121  }
122  maxarg1=anorm;
123  maxarg2=(fabs(w(i))+fabs(rv1.at(i-1)));
124  anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
125 
126  }
127 
128  // Accumulation of right-hand transformation
129  for (i= cols ; i>=1; i--) {
130  if ( i < cols ) {
131  if (g) {
132  for ( j=ppi; j <= cols; j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
133  for ( j=ppi; j <= cols; j++) {
134  for (s=0.0,k=ppi;k <= cols;k++) s += U(i,k)*V(k,j);
135  for ( k=ppi; k <= cols;k++) V(k,j) += s*V(k,i);
136  }
137  }
138  for( j=ppi; j<=cols ; j++) V(i,j)=V(j,i)=0.0;
139  }
140  V(i,i)=1.0;
141  g=rv1.at(i-1);
142  ppi=i;
143  }
144 
145  // Accumulation of left-hand transformation
146  for (i= cols < rows ? cols: rows; i>=1; i--) {
147  ppi=i+1;
148  g=w(i);
149  for( j=ppi; j<=cols ; j++) U(i,j)=0.0;
150  if (g) {
151  g=1.0/g;
152  for ( j=ppi; j <= cols; j++) {
153  for( s=0.0, k=ppi; k<=rows ; k++) s += U(k,i)*U(k,j);
154  f=(s/U(i,i))*g;
155  for ( k=i; k <= rows;k++) U(k,j) += f*U(k,i);
156  }
157  for( j=i; j<=rows ; j++) U(j,i) *=g;
158  } else {
159  for( j=i; j<=rows ; j++) U(j,i) = 0.0;
160  }
161  ++U(i,i);
162  }
163 
164  // Diagonalization of the bidiagonal form:
165  // Loop over singular values,and over allowed iterations
166  for ( k=cols; k >= 1; k--) {
167  for (its=1; its <= maxIter; its++) {
168  flag=true;
169  //Test for splitting. Note that rv1[i] is always 0
170  for (ppi=k; ppi >= 1; ppi--) {
171  nm=ppi-1;
172  if ((fabs(rv1.at(ppi-1))+anorm) == anorm) {
173  flag=false;
174  break;
175  }
176  if ((fabs(w(nm)+anorm) == anorm)) {
177  break;
178  }
179  }
180  //Cancellation of rv1[ppi],if ppi>1.
181  if (flag) {
182  c=0.0;
183  s=1.0;
184  for ( i=ppi; i <= k ;i++) {
185  f=s*rv1.at(i-1);
186  rv1.at(i-1)=c*rv1.at(i-1);
187  if ((fabs(f)+anorm) == anorm) {
188  break;
189  }
190  g=w(i);
191  h=PYTHAG(f,g);
192  w(i)=h;
193  h=1.0/h;
194  c=g*h;
195  s=-f*h;
196  for ( j=1;j <= rows; j++) {
197  y=U(j,nm);
198  z=U(j,i);
199  U(j,nm)=y*c+z*s;
200  U(j,i)=z*c-y*s;
201  }
202  }
203  }
204  z=w(k);
205 
206  // Convergence. Singular value is made nonnegative.
207  if (ppi == k) {
208  if (z < 0.0) {
209  w(k)=-z;
210  for (j=1; j <= cols; j++) V(j,k)=-V(j,k);
211  }
212  break;
213  }
214 
215  if (its == maxIter) {
216  //char x[80];
217  std::cout << "SVD did not converge after " << maxIter << " iterations!" << std::endl;
218  // make all singular values zero -> rank 0
219  w = 0.0;
220  return false;
221  }
222  // shift from bottom 2-by-2 minor
223  x=w(ppi);
224  nm=k-1;
225  y=w(nm);
226  g=rv1.at(nm-1);
227  h=rv1.at(k-1);
228  f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
229 
230  g = PYTHAG(f,1.0);
231  f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
232 
233  //Next QR transformation
234  c=s=1.0;
235  for ( j=ppi; j<=nm ;j++){
236  i=j+1;
237  g=rv1.at(i-1);
238  y=w(i);
239  h=s*g;
240  g=c*g;
241  z=PYTHAG(f,h);
242  rv1.at(j-1)=z;
243  c=f/z;
244  s=h/z;
245  f=x*c+g*s;
246  g=g*c-x*s;
247  h=y*s;
248  y*=c;
249  for (jj=1; jj<=cols ;jj++){
250  x=V(jj,j);
251  z=V(jj,i);
252  V(jj,j)=x*c+z*s;
253  V(jj,i)=z*c-x*s;
254  }
255  z=PYTHAG(f,h);
256  w(j)=z;
257 
258  if (z) {
259  z=1.0/z;
260  c=f*z;
261  s=h*z;
262  }
263  f=c*g+s*y;
264  x=c*y-s*g;
265  for (jj=1; jj<=rows; jj++){
266  y=U(jj,j);
267  z=U(jj,i);
268  U(jj,j)=y*c+z*s;
269  U(jj,i)=z*c-y*s;
270  }
271  }
272  rv1.at(ppi-1)=0.0;
273  rv1.at(k-1)=f;
274  w(k)=x;
275 
276  }
277  }
278  return true;
279  }
280 
281 
282  double
283  Matrix_Wrapper::PYTHAG(double a,double b) const
284  {
285  double at,bt,ct;
286  at = fabs(a);
287  bt = fabs(b);
288  if (at > bt ) {
289  ct=bt/at;
290  return at*sqrt(1.0+ct*ct);
291  } else {
292  if (bt==0)
293  return 0.0;
294  else {
295  ct=at/bt;
296  return bt*sqrt(1.0+ct*ct);
297  }
298  }
299  }
300 
301 
302  double
303  Matrix_Wrapper::SIGN(double a,double b) const
304  {
305  return ((b) >= 0.0 ? fabs(a) : -fabs(a));
306  }
307 
308  // See <http://dsp.ee.sun.ac.za/~schwardt/dsp813/lecture10/node7.html>
309  MyMatrix
310  Matrix_Wrapper::pseudoinverse(double epsilon) const
311  {
312  int rows;
313  rows = this->rows();
314  int cols = this->columns();
315  // calculate SVD decomposition
316  MyMatrix U,V;
317  MyColumnVector D;
318 
319  bool res;
320  res = SVD(D,U,V); // U=mxn D=n V=nxn
321  assert(res);
322 
323  Matrix Dinv(cols,cols);
324  Dinv = 0;
325  for (unsigned int i=0; i<D.rows(); i++)
326  if ( D(i+1) < epsilon )
327  Dinv(i+1,i+1) = 0;
328  else
329  Dinv(i+1,i+1) = 1/D(i+1);
330 
331  #ifdef __DEBUG__
332  std::cout << "MATRIX::pseudoinverse() Dinv =\n" << Dinv << std::endl;
333  #endif //__DEBUG__
334 
335  return V * Dinv * U.transpose();
336  }
337 
338 } //namespace
#define MyMatrix
f
#define MySymmetricMatrix
#define MyColumnVector


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 28 2022 21:56:33