orocos_kdl
svd_eigen_HH.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "svd_eigen_HH.hpp"
23 
24 namespace KDL{
25 
26  int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon)
27  {
28  //get the rows/columns of the matrix
29  const int rows = static_cast<int>(A.rows());
30  const int cols = static_cast<int>(A.cols());
31 
32  U.setZero();
33  U.topLeftCorner(rows,cols)=A;
34 
35  int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
36  int ppi(0);
37  bool flag,maxarg1,maxarg2;
38  double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
39 
40  /* Householder reduction to bidiagonal form. */
41  for (i=0;i<cols;i++) {
42  ppi=i+1;
43  tmp(i)=scale*g;
44  g=s=scale=0.0;
45  if (i<rows) {
46  // compute the sum of the i-th column, starting from the i-th row
47  for (k=i;k<rows;k++) scale += fabs(U(k,i));
48  if (fabs(scale)>epsilon) {
49  // multiply the i-th column by 1.0/scale, start from the i-th element
50  // sum of squares of column i, start from the i-th element
51  for (k=i;k<rows;k++) {
52  U(k,i) /= scale;
53  s += U(k,i)*U(k,i);
54  }
55  f=U(i,i); // f is the diag elem
56  if (!(s>=0)) return -3;
57  g = -SIGN(sqrt(s),f);
58  h=f*g-s;
59  U(i,i)=f-g;
60  for (j=ppi;j<cols;j++) {
61  // dot product of columns i and j, starting from the i-th row
62  for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
63  if (!(h!=0)) return -4;
64  f=s/h;
65  // copy the scaled i-th column into the j-th column
66  for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
67  }
68  for (k=i;k<rows;k++) U(k,i) *= scale;
69  }
70  }
71  // save singular value
72  S(i)=scale*g;
73  g=s=scale=0.0;
74  if ((i <rows) && (i+1 != cols)) {
75  // sum of row i, start from columns i+1
76  for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
77  if (fabs(scale)>epsilon) {
78  for (k=ppi;k<cols;k++) {
79  U(i,k) /= scale;
80  s += U(i,k)*U(i,k);
81  }
82  f=U(i,ppi);
83  if (!(s>=0)) return -5;
84  g = -SIGN(sqrt(s),f);
85  h=f*g-s;
86  U(i,ppi)=f-g;
87  if (!(h!=0)) return -6;
88  for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
89  for (j=ppi;j<rows;j++) {
90  for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
91  for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
92  }
93  for (k=ppi;k<cols;k++) U(i,k) *= scale;
94  }
95  }
96  maxarg1=anorm;
97  maxarg2=(fabs(S(i))+fabs(tmp(i)));
98  anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
99  }
100  /* Accumulation of right-hand transformations. */
101  for (i=cols-1;i>=0;i--) {
102  if (i<cols-1) {
103  if (fabs(g)>epsilon) {
104  if (!(U(i,ppi)!=0)) return -7;
105  for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
106  for (j=ppi;j<cols;j++) {
107  for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
108  for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
109  }
110  }
111  for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
112  }
113  V(i,i)=1.0;
114  g=tmp(i);
115  ppi=i;
116  }
117  /* Accumulation of left-hand transformations. */
118  for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
119  ppi=i+1;
120  g=S(i);
121  for (j=ppi;j<cols;j++) U(i,j)=0.0;
122  if (fabs(g)>epsilon) {
123  g=1.0/g;
124  for (j=ppi;j<cols;j++) {
125  for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
126  if (!(U(i,i)!=0)) return -8;
127  f=(s/U(i,i))*g;
128  for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
129  }
130  for (j=i;j<rows;j++) U(j,i) *= g;
131  } else {
132  for (j=i;j<rows;j++) U(j,i)=0.0;
133  }
134  ++U(i,i);
135  }
136 
137  /* Diagonalization of the bidiagonal form. */
138  for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
139  for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */
140  flag=true;
141  for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */
142  nm=ppi-1; /* Note that tmp[1] is always zero. */
143  if ((fabs(tmp(ppi))+anorm) == anorm) {
144  flag=false;
145  break;
146  }
147  if ((fabs(S(nm)+anorm) == anorm)) break;
148  }
149  if (flag) {
150  c=0.0; /* Cancellation of tmp[l], if l>1: */
151  s=1.0;
152  for (i=ppi;i<=k;i++) {
153  f=s*tmp(i);
154  tmp(i)=c*tmp(i);
155  if ((fabs(f)+anorm) == anorm) break;
156  g=S(i);
157  h=PYTHAG(f,g);
158  S(i)=h;
159  if (!(h!=0)) return -9;
160  h=1.0/h;
161  c=g*h;
162  s=(-f*h);
163  for (j=0;j<rows;j++) {
164  y=U(j,nm);
165  z=U(j,i);
166  U(j,nm)=y*c+z*s;
167  U(j,i)=z*c-y*s;
168  }
169  }
170  }
171  z=S(k);
172 
173  if (ppi == k) { /* Convergence. */
174  if (z < 0.0) { /* Singular value is made nonnegative. */
175  S(k) = -z;
176  for (j=0;j<cols;j++) V(j,k)=-V(j,k);
177  }
178  break;
179  }
180 
181  x=S(ppi); /* Shift from bottom 2-by-2 minor: */
182  nm=k-1;
183  y=S(nm);
184  g=tmp(nm);
185  h=tmp(k);
186  if (!(h!=0&&y!=0)) return -10;
187  f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
188 
189  g=PYTHAG(f,1.0);
190  if (!(x!=0)) return -11;
191  if (!((f+SIGN(g,f))!=0)) return -12;
192  f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
193 
194  /* Next QR transformation: */
195  c=s=1.0;
196  for (j=ppi;j<=nm;j++) {
197  i=j+1;
198  g=tmp(i);
199  y=S(i);
200  h=s*g;
201  g=c*g;
202  z=PYTHAG(f,h);
203  tmp(j)=z;
204  if (!(z!=0)) return -13;
205  c=f/z;
206  s=h/z;
207  f=x*c+g*s;
208  g=g*c-x*s;
209  h=y*s;
210  y=y*c;
211  for (jj=0;jj<cols;jj++) {
212  x=V(jj,j);
213  z=V(jj,i);
214  V(jj,j)=x*c+z*s;
215  V(jj,i)=z*c-x*s;
216  }
217  z=PYTHAG(f,h);
218  S(j)=z;
219  if (fabs(z)>epsilon) {
220  z=1.0/z;
221  c=f*z;
222  s=h*z;
223  }
224  f=(c*g)+(s*y);
225  x=(c*y)-(s*g);
226  for (jj=0;jj<rows;jj++) {
227  y=U(jj,j);
228  z=U(jj,i);
229  U(jj,j)=y*c+z*s;
230  U(jj,i)=z*c-y*s;
231  }
232  }
233  tmp(ppi)=0.0;
234  tmp(k)=f;
235  S(k)=x;
236  }
237  }
238 
239  //Sort eigen values:
240  for (i=0; i<cols; i++){
241 
242  double S_max = S(i);
243  int i_max = i;
244  for (j=i+1; j<cols; j++){
245  double Sj = S(j);
246  if (Sj > S_max){
247  S_max = Sj;
248  i_max = j;
249  }
250  }
251  if (i_max != i){
252  /* swap eigenvalues */
253  double tmp = S(i);
254  S(i)=S(i_max);
255  S(i_max)=tmp;
256 
257  /* swap eigenvectors */
258  U.col(i).swap(U.col(i_max));
259  V.col(i).swap(V.col(i_max));
260  }
261  }
262 
263  if (its == maxiter)
264  return (-2);
265  else
266  return (0);
267  }
268 }
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
KDL::svd_eigen_HH
int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon)
Definition: svd_eigen_HH.cpp:26
KDL::PYTHAG
double PYTHAG(double a, double b)
Definition: svd_eigen_HH.hpp:33
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::SIGN
double SIGN(double a, double b)
Definition: svd_eigen_HH.hpp:51
svd_eigen_HH.hpp
Generated on Thu Nov 14 2024 04:37:48 for orocos_kdl by   doxygen 1.8.17