Blender  V3.3
svd_eigen_HH.hpp
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 #ifndef SVD_EIGEN_HH_HPP
25 #define SVD_EIGEN_HH_HPP
26 
27 
28 #include <Eigen/Core>
29 #include <algorithm>
30 
31 namespace KDL
32 {
33  template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
34  double at,bt,ct;
35  at = fabs(a);
36  bt = fabs(b);
37  if (at > bt ) {
38  ct=bt/at;
39  return Scalar(at*sqrt(1.0+ct*ct));
40  } else {
41  if (bt==0)
42  return Scalar(0.0);
43  else {
44  ct=at/bt;
45  return Scalar(bt*sqrt(1.0+ct*ct));
46  }
47  }
48  }
49 
50 
51  template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
52  return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
53  }
54 
67  template<typename MatrixA, typename MatrixUV, typename VectorS>
69  const Eigen::MatrixBase<MatrixA>& A,
70  Eigen::MatrixBase<MatrixUV>& U,
71  Eigen::MatrixBase<VectorS>& S,
72  Eigen::MatrixBase<MatrixUV>& V,
73  Eigen::MatrixBase<VectorS>& tmp,
74  int maxiter=150)
75  {
76  //get the rows/columns of the matrix
77  const int rows = A.rows();
78  const int cols = A.cols();
79 
80  U = A;
81 
82  int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
83  int ppi(0);
84  bool flag;
85  e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
86 
87  g=scale=anorm=e_scalar(0.0);
88 
89  /* Householder reduction to bidiagonal form. */
90  for (i=0;i<cols;i++) {
91  ppi=i+1;
92  tmp(i)=scale*g;
93  g=s=scale=e_scalar(0.0);
94  if (i<rows) {
95  // compute the sum of the i-th column, starting from the i-th row
96  for (k=i;k<rows;k++) scale += fabs(U(k,i));
97  if (scale!=0) {
98  // multiply the i-th column by 1.0/scale, start from the i-th element
99  // sum of squares of column i, start from the i-th element
100  for (k=i;k<rows;k++) {
101  U(k,i) /= scale;
102  s += U(k,i)*U(k,i);
103  }
104  f=U(i,i); // f is the diag elem
105  g = -SIGN(e_scalar(sqrt(s)),f);
106  h=f*g-s;
107  U(i,i)=f-g;
108  for (j=ppi;j<cols;j++) {
109  // dot product of columns i and j, starting from the i-th row
110  for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
111  f=s/h;
112  // copy the scaled i-th column into the j-th column
113  for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
114  }
115  for (k=i;k<rows;k++) U(k,i) *= scale;
116  }
117  }
118  // save singular value
119  S(i)=scale*g;
120  g=s=scale=e_scalar(0.0);
121  if ((i <rows) && (i+1 != cols)) {
122  // sum of row i, start from columns i+1
123  for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
124  if (scale!=0) {
125  for (k=ppi;k<cols;k++) {
126  U(i,k) /= scale;
127  s += U(i,k)*U(i,k);
128  }
129  f=U(i,ppi);
130  g = -SIGN(e_scalar(sqrt(s)),f);
131  h=f*g-s;
132  U(i,ppi)=f-g;
133  for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
134  for (j=ppi;j<rows;j++) {
135  for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
136  for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
137  }
138  for (k=ppi;k<cols;k++) U(i,k) *= scale;
139  }
140  }
141  maxarg1=anorm;
142  maxarg2=(fabs(S(i))+fabs(tmp(i)));
143  anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
144  }
145  /* Accumulation of right-hand transformations. */
146  for (i=cols-1;i>=0;i--) {
147  if (i<cols-1) {
148  if (g) {
149  for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
150  for (j=ppi;j<cols;j++) {
151  for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
152  for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
153  }
154  }
155  for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
156  }
157  V(i,i)=1.0;
158  g=tmp(i);
159  ppi=i;
160  }
161  /* Accumulation of left-hand transformations. */
162  for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
163  ppi=i+1;
164  g=S(i);
165  for (j=ppi;j<cols;j++) U(i,j)=0.0;
166  if (g) {
167  g=e_scalar(1.0)/g;
168  for (j=ppi;j<cols;j++) {
169  for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
170  f=(s/U(i,i))*g;
171  for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
172  }
173  for (j=i;j<rows;j++) U(j,i) *= g;
174  } else {
175  for (j=i;j<rows;j++) U(j,i)=0.0;
176  }
177  ++U(i,i);
178  }
179 
180  /* Diagonalization of the bidiagonal form. */
181  for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
182  for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */
183  flag=true;
184  for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */
185  nm=ppi-1; /* Note that tmp(1) is always zero. */
186  if ((fabs(tmp(ppi))+anorm) == anorm) {
187  flag=false;
188  break;
189  }
190  if ((fabs(S(nm)+anorm) == anorm)) break;
191  }
192  if (flag) {
193  c=e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */
194  s=e_scalar(1.);
195  for (i=ppi;i<=k;i++) {
196  f=s*tmp(i);
197  tmp(i)=c*tmp(i);
198  if ((fabs(f)+anorm) == anorm) break;
199  g=S(i);
200  h=PYTHAG(f,g);
201  S(i)=h;
202  h=e_scalar(1.0)/h;
203  c=g*h;
204  s=(-f*h);
205  for (j=0;j<rows;j++) {
206  y=U(j,nm);
207  z=U(j,i);
208  U(j,nm)=y*c+z*s;
209  U(j,i)=z*c-y*s;
210  }
211  }
212  }
213  z=S(k);
214 
215  if (ppi == k) { /* Convergence. */
216  if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */
217  S(k) = -z;
218  for (j=0;j<cols;j++) V(j,k)=-V(j,k);
219  }
220  break;
221  }
222 
223  x=S(ppi); /* Shift from bottom 2-by-2 minor: */
224  nm=k-1;
225  y=S(nm);
226  g=tmp(nm);
227  h=tmp(k);
228  f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);
229 
230  g=PYTHAG(f,e_scalar(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=tmp(i);
238  y=S(i);
239  h=s*g;
240  g=c*g;
241  z=PYTHAG(f,h);
242  tmp(j)=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=y*c;
249  for (jj=0;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  S(j)=z;
257  if (z) {
258  z=e_scalar(1.0)/z;
259  c=f*z;
260  s=h*z;
261  }
262  f=(c*g)+(s*y);
263  x=(c*y)-(s*g);
264  for (jj=0;jj<rows;jj++) {
265  y=U(jj,j);
266  z=U(jj,i);
267  U(jj,j)=y*c+z*s;
268  U(jj,i)=z*c-y*s;
269  }
270  }
271  tmp(ppi)=0.0;
272  tmp(k)=f;
273  S(k)=x;
274  }
275  }
276 
277  //Sort eigen values:
278  for (i=0; i<cols; i++){
279 
280  double S_max = S(i);
281  int i_max = i;
282  for (j=i+1; j<cols; j++){
283  double Sj = S(j);
284  if (Sj > S_max){
285  S_max = Sj;
286  i_max = j;
287  }
288  }
289  if (i_max != i){
290  /* swap eigenvalues */
291  e_scalar tmp = S(i);
292  S(i)=S(i_max);
293  S(i_max)=tmp;
294 
295  /* swap eigenvectors */
296  U.col(i).swap(U.col(i_max));
297  V.col(i).swap(V.col(i_max));
298  }
299  }
300 
301 
302  if (its == maxiter)
303  return (-2);
304  else
305  return (0);
306  }
307 
308 }
309 #endif
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
#define U
#define A
unsigned int U
Definition: btGjkEpa3.h:78
#define e_scalar
Definition: eigen_types.hpp:37
float Scalar
Definition: eigen_utils.h:26
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
Definition: chain.cpp:27
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:367
Scalar PYTHAG(Scalar a, Scalar b)
int svd_eigen_HH(const Eigen::MatrixBase< MatrixA > &A, Eigen::MatrixBase< MatrixUV > &U, Eigen::MatrixBase< VectorS > &S, Eigen::MatrixBase< MatrixUV > &V, Eigen::MatrixBase< VectorS > &tmp, int maxiter=150)
Scalar SIGN(Scalar a, Scalar b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
static const pxr::TfToken g("g", pxr::TfToken::Immortal)
CCL_NAMESPACE_BEGIN struct Window V