svd_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 
23 //Based on the svd of the KDL-0.2 library by Erwin Aertbelien
24 
25 #include "svd_HH.hpp"
26 
27 namespace KDL
28 {
29  inline double PYTHAG(double a,double b) {
30  double at,bt,ct;
31  at = fabs(a);
32  bt = fabs(b);
33  if (at > bt ) {
34  ct=bt/at;
35  return at*sqrt(1.0+ct*ct);
36  } else {
37  if (bt==0)
38  return 0.0;
39  else {
40  ct=at/bt;
41  return bt*sqrt(1.0+ct*ct);
42  }
43  }
44  }
45 
46 
47  inline double SIGN(double a,double b) {
48  return ((b) >= 0.0 ? fabs(a) : -fabs(a));
49  }
50 
51  SVD_HH::SVD_HH(const Jacobian& jac):
52  tmp(jac.columns())
53  {
54  }
55 
56  SVD_HH::~SVD_HH()
57  {
58  }
59 
60  int SVD_HH::calculate(const Jacobian& jac,std::vector<JntArray>& U,JntArray& w,std::vector<JntArray>& v,int maxiter)
61  {
62 
63  //get the rows/columns of the jacobian
64  const int rows = jac.rows();
65  const int cols = jac.columns();
66 
67  int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
68  int ppi(0);
69  bool flag,maxarg1,maxarg2;
70  double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
71 
72  for(i=0;i<rows;i++)
73  for(j=0;j<cols;j++)
74  U[i](j)=jac(i,j);
75  if(rows>cols)
76  for(i=rows;i<cols;i++)
77  for(j=0;j<cols;j++)
78  U[i](j)=0;
79 
80  /* Householder reduction to bidiagonal form. */
81  for (i=0;i<cols;i++) {
82  ppi=i+1;
83  tmp(i)=scale*g;
84  g=s=scale=0.0;
85  if (i<rows) {
86  for (k=i;k<rows;k++) scale += fabs(U[k](i));
87  if (scale) {
88  // multiply the i-th column by 1.0/scale, start from the i-th element
89  // sum of squares of column i, start from the i-th element
90  for (k=i;k<rows;k++) {
91  U[k](i) /= scale;
92  s += U[k](i)*U[k](i);
93  }
94  f=U[i](i); // f is the diag elem
95  g = -SIGN(sqrt(s),f);
96  h=f*g-s;
97  U[i](i)=f-g;
98  for (j=ppi;j<cols;j++) {
99  // dot product of columns i and j, starting from the i-th row
100  for (s=0.0,k=i;k<rows;k++) s += U[k](i)*U[k](j);
101  f=s/h;
102  // copy the scaled i-th column into the j-th column
103  for (k=i;k<rows;k++) U[k](j) += f*U[k](i);
104  }
105  for (k=i;k<rows;k++) U[k](i) *= scale;
106  }
107  }
108  // save singular value
109  w(i)=scale*g;
110  g=s=scale=0.0;
111  if ((i <rows) && (i+1 != cols)) {
112  // sum of row i, start from columns i+1
113  for (k=ppi;k<cols;k++) scale += fabs(U[i](k));
114  if (scale) {
115  for (k=ppi;k<cols;k++) {
116  U[i](k) /= scale;
117  s += U[i](k)*U[i](k);
118  }
119  f=U[i](ppi);
120  g = -SIGN(sqrt(s),f);
121  h=f*g-s;
122  U[i](ppi)=f-g;
123  for (k=ppi;k<cols;k++) tmp(k)=U[i](k)/h;
124  for (j=ppi;j<rows;j++) {
125  for (s=0.0,k=ppi;k<cols;k++) s += U[j](k)*U[i](k);
126  for (k=ppi;k<cols;k++) U[j](k) += s*tmp(k);
127  }
128  for (k=ppi;k<cols;k++) U[i](k) *= scale;
129  }
130  }
131  maxarg1=anorm;
132  maxarg2=(fabs(w(i))+fabs(tmp(i)));
133  anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
134  }
135  /* Accumulation of right-hand transformations. */
136  for (i=cols-1;i>=0;i--) {
137  if (i<cols-1) {
138  if (g) {
139  for (j=ppi;j<cols;j++) v[j](i)=(U[i](j)/U[i](ppi))/g;
140  for (j=ppi;j<cols;j++) {
141  for (s=0.0,k=ppi;k<cols;k++) s += U[i](k)*v[k](j);
142  for (k=ppi;k<cols;k++) v[k](j) += s*v[k](i);
143  }
144  }
145  for (j=ppi;j<cols;j++) v[i](j)=v[j](i)=0.0;
146  }
147  v[i](i)=1.0;
148  g=tmp(i);
149  ppi=i;
150  }
151  /* Accumulation of left-hand transformations. */
152  for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
153  ppi=i+1;
154  g=w(i);
155  for (j=ppi;j<cols;j++) U[i](j)=0.0;
156  if (g) {
157  g=1.0/g;
158  for (j=ppi;j<cols;j++) {
159  for (s=0.0,k=ppi;k<rows;k++) s += U[k](i)*U[k](j);
160  f=(s/U[i](i))*g;
161  for (k=i;k<rows;k++) U[k](j) += f*U[k](i);
162  }
163  for (j=i;j<rows;j++) U[j](i) *= g;
164  } else {
165  for (j=i;j<rows;j++) U[j](i)=0.0;
166  }
167  ++U[i](i);
168  }
169 
170  /* Diagonalization of the bidiagonal form. */
171  for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
172  for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */
173  flag=true;
174  for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */
175  nm=ppi-1; /* Note that tmp[1] is always zero. */
176  if ((fabs(tmp(ppi))+anorm) == anorm) {
177  flag=false;
178  break;
179  }
180  if ((fabs(w(nm)+anorm) == anorm)) break;
181  }
182  if (flag) {
183  c=0.0; /* Cancellation of tmp[l], if l>1: */
184  s=1.0;
185  for (i=ppi;i<=k;i++) {
186  f=s*tmp(i);
187  tmp(i)=c*tmp(i);
188  if ((fabs(f)+anorm) == anorm) break;
189  g=w(i);
190  h=PYTHAG(f,g);
191  w(i)=h;
192  h=1.0/h;
193  c=g*h;
194  s=(-f*h);
195  for (j=0;j<rows;j++) {
196  y=U[j](nm);
197  z=U[j](i);
198  U[j](nm)=y*c+z*s;
199  U[j](i)=z*c-y*s;
200  }
201  }
202  }
203  z=w(k);
204 
205  if (ppi == k) { /* Convergence. */
206  if (z < 0.0) { /* Singular value is made nonnegative. */
207  w(k) = -z;
208  for (j=0;j<cols;j++) v[j](k)=-v[j](k);
209  }
210  break;
211  }
212  x=w(ppi); /* Shift from bottom 2-by-2 minor: */
213  nm=k-1;
214  y=w(nm);
215  g=tmp(nm);
216  h=tmp(k);
217  f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
218 
219  g=PYTHAG(f,1.0);
220  f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
221 
222  /* Next QR transformation: */
223  c=s=1.0;
224  for (j=ppi;j<=nm;j++) {
225  i=j+1;
226  g=tmp(i);
227  y=w(i);
228  h=s*g;
229  g=c*g;
230  z=PYTHAG(f,h);
231  tmp(j)=z;
232  c=f/z;
233  s=h/z;
234  f=x*c+g*s;
235  g=g*c-x*s;
236  h=y*s;
237  y=y*c;
238  for (jj=0;jj<cols;jj++) {
239  x=v[jj](j);
240  z=v[jj](i);
241  v[jj](j)=x*c+z*s;
242  v[jj](i)=z*c-x*s;
243  }
244  z=PYTHAG(f,h);
245  w(j)=z;
246  if (z) {
247  z=1.0/z;
248  c=f*z;
249  s=h*z;
250  }
251  f=(c*g)+(s*y);
252  x=(c*y)-(s*g);
253  for (jj=0;jj<rows;jj++) {
254  y=U[jj](j);
255  z=U[jj](i);
256  U[jj](j)=y*c+z*s;
257  U[jj](i)=z*c-y*s;
258  }
259  }
260  tmp(ppi)=0.0;
261  tmp(k)=f;
262  w(k)=x;
263  }
264  }
265  if (its == maxiter)
266  return (-2);
267  else
268  return (0);
269  }
270 
271 }
272 
273 
274 
275 
KDL::SIGN
double SIGN(double a, double b)
Definition: svd_eigen_HH.hpp:53
KDL::JntArray
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:75
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
KDL::SVD_HH::SVD_HH
SVD_HH(const Jacobian &jac)
Definition: svd_HH.cpp:51
KDL::SVD_HH::~SVD_HH
~SVD_HH()
Definition: svd_HH.cpp:56
KDL::SVD_HH::tmp
JntArray tmp
Definition: svd_HH.hpp:41
KDL::PYTHAG
double PYTHAG(double a, double b)
Definition: svd_eigen_HH.hpp:35
KDL::SVD_HH::calculate
int calculate(const Jacobian &jac, std::vector< JntArray > &U, JntArray &w, std::vector< JntArray > &v, int maxiter)
Definition: svd_HH.cpp:60
svd_HH.hpp
KDL::Jacobian::rows
unsigned int rows() const
Definition: jacobian.cpp:70

orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36