cgv
Loading...
Searching...
No Matches
svd.h
1#pragma once
2
3#include "mat.h"
4#include "diag_mat.h"
5#include "functions.h"
6
7#include <limits>
8#include <algorithm>
9
10namespace cgv {
11namespace math {
12
13//helper funcition for svd
14template <typename T>
15T pythag(const T a, const T b) {
16 T absa=std::abs(a), absb=std::abs(b);
17 return T((absa > absb ? absa*sqrt(1.0+(absb/absa)*(absb/absa)) :
18 (absb == 0.0 ? 0.0 : absb*sqrt(1.0+(absa/absb)*(absa/absb)))));
19 }
20
28template <typename T>
29bool svd(const mat<T> &a, mat<T> &u, diag_mat<T> &w, mat<T> &v,bool ordering=true, int maxiter=30)
30{
31 int m = a.nrows();
32 int n = a.ncols();
33
34 u=a;
35 v.resize(n,n);
36 w.resize(n);
37 T eps=std::numeric_limits<T>::epsilon();
38 //decompose
39 bool flag;
40 int i,its,j,jj,k,l,nm;
41 T anorm,c,f,g,h,s,scale,x,y,z;
42 vec<T> rv1(n);
43 g = scale = anorm = 0.0;
44 for (i=0;i<n;i++) {
45 l=i+2;
46 rv1[i]=scale*g;
47 g=s=scale=0.0;
48 if (i < m) {
49 for (k=i;k<m;k++) scale += std::abs(u(k,i));
50 if (scale != 0.0) {
51 for (k=i;k<m;k++) {
52 u(k,i) /= scale;
53 s += u(k,i)*u(k,i);
54 }
55 f=u(i,i);
56 g = -sign(std::sqrt(s),f);
57 h=f*g-s;
58 u(i,i)=f-g;
59 for (j=l-1;j<n;j++) {
60 for (s=0.0,k=i;k<m;k++) s += u(k,i)*u(k,j);
61 f=s/h;
62 for (k=i;k<m;k++) u(k,j) += f*u(k,i);
63 }
64 for (k=i;k<m;k++) u(k,i) *= scale;
65 }
66 }
67 w[i]=scale *g;
68 g=s=scale=0.0;
69 if (i+1 <= m && i+1 != n) {
70 for (k=l-1;k<n;k++) scale += std::abs(u(i,k));
71 if (scale != 0.0) {
72 for (k=l-1;k<n;k++) {
73 u(i,k) /= scale;
74 s += u(i,k)*u(i,k);
75 }
76 f=u(i,l-1);
77 g = -sign(std::sqrt(s),f);
78 h=f*g-s;
79 u(i,l-1)=f-g;
80 for (k=l-1;k<n;k++) rv1[k]=u(i,k)/h;
81 for (j=l-1;j<m;j++) {
82 for (s=0.0,k=l-1;k<n;k++) s += u(j,k)*u(i,k);
83 for (k=l-1;k<n;k++) u(j,k) += s*rv1[k];
84 }
85 for (k=l-1;k<n;k++) u(i,k) *= scale;
86 }
87 }
88 anorm=(std::max)(anorm,(std::abs(w[i])+std::abs(rv1[i])));
89 }
90 for (i=n-1;i>=0;i--) {
91 if (i < n-1) {
92 if (g != 0.0) {
93 for (j=l;j<n;j++)
94 v(j,i)=(u(i,j)/u(i,l))/g;
95 for (j=l;j<n;j++) {
96 for (s=0.0,k=l;k<n;k++) s += u(i,k)*v(k,j);
97 for (k=l;k<n;k++) v(k,j) += s*v(k,i);
98 }
99 }
100 for (j=l;j<n;j++) v(i,j)=v(j,i)=0.0;
101 }
102 v(i,i)=1.0;
103 g=rv1[i];
104 l=i;
105 }
106 for (i=(std::min)(m,n)-1;i>=0;i--) {
107 l=i+1;
108 g=w[i];
109 for (j=l;j<n;j++) u(i,j)=0.0;
110 if (g != 0.0) {
111 g=(T)1.0/g;
112 for (j=l;j<n;j++) {
113 for (s=0.0,k=l;k<m;k++) s += u(k,i)*u(k,j);
114 f=(s/u(i,i))*g;
115 for (k=i;k<m;k++) u(k,j) += f*u(k,i);
116 }
117 for (j=i;j<m;j++) u(j,i) *= g;
118 } else for (j=i;j<m;j++) u(j,i)=0.0;
119 ++u(i,i);
120 }
121 for (k=n-1;k>=0;k--) {
122 for (its=0;its<maxiter;its++) {
123 flag=true;
124 for (l=k;l>=0;l--) {
125 nm=l-1;
126 if (l == 0 || std::abs(rv1[l]) <= eps*anorm) {
127 flag=false;
128 break;
129 }
130 if (std::abs(w[nm]) <= eps*anorm) break;
131 }
132 if (flag) {
133 c=0.0;
134 s=1.0;
135 for (i=l;i<k+1;i++) {
136 f=s*rv1[i];
137 rv1[i]=c*rv1[i];
138 if (std::abs(f) <= eps*anorm) break;
139 g=w[i];
140 h=pythag(f,g);
141 w[i]=h;
142 h=(T)1.0/h;
143 c=g*h;
144 s = -f*h;
145 for (j=0;j<m;j++) {
146 y=u(j,nm);
147 z=u(j,i);
148 u(j,nm)=y*c+z*s;
149 u(j,i)=z*c-y*s;
150 }
151 }
152 }
153 z=w[k];
154 if (l == k) {
155 if (z < 0.0) {
156 w[k] = -z;
157 for (j=0;j<n;j++) v(j,k) = -v(j,k);
158 }
159 break;
160 }
161 if (its == maxiter-1)
162 return false;
163 x=w[l];
164 nm=k-1;
165 y=w[nm];
166 g=rv1[nm];
167 h=rv1[k];
168 f=((y-z)*(y+z)+(g-h)*(g+h))/((T)2.0*h*y);
169 g=pythag(f,(T)1.0);
170 f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x;
171 c=s=1.0;
172 for (j=l;j<=nm;j++) {
173 i=j+1;
174 g=rv1[i];
175 y=w[i];
176 h=s*g;
177 g=c*g;
178 z=pythag(f,h);
179 rv1[j]=z;
180 c=f/z;
181 s=h/z;
182 f=x*c+g*s;
183 g=g*c-x*s;
184 h=y*s;
185 y *= c;
186 for (jj=0;jj<n;jj++) {
187 x=v(jj,j);
188 z=v(jj,i);
189 v(jj,j)=x*c+z*s;
190 v(jj,i)=z*c-x*s;
191 }
192 z=pythag(f,h);
193 w[j]=z;
194 if (z) {
195 z=(T)1.0/z;
196 c=f*z;
197 s=h*z;
198 }
199 f=c*g+s*y;
200 x=c*y-s*g;
201 for (jj=0;jj<m;jj++) {
202 y=u(jj,j);
203 z=u(jj,i);
204 u(jj,j)=y*c+z*s;
205 u(jj,i)=z*c-y*s;
206 }
207 }
208 rv1[l]=0.0;
209 rv1[k]=f;
210 w[k]=x;
211 }
212 }
213 if(ordering)
214 {
215 int i,j,k,s,inc=1;
216 T sw;
217 vec<T> su(m), sv(n);
218 do { inc *= 3; inc++; } while (inc <= n);
219 do {
220 inc /= 3;
221 for (i=inc;i<n;i++) {
222 sw = w[i];
223 for (k=0;k<m;k++) su[k] = u(k,i);
224 for (k=0;k<n;k++) sv[k] = v(k,i);
225 j = i;
226 while (w[j-inc] < sw) {
227 w[j] = w[j-inc];
228 for (k=0;k<m;k++) u(k,j) = u(k,j-inc);
229 for (k=0;k<n;k++) v(k,j) = v(k,j-inc);
230 j -= inc;
231 if (j < inc) break;
232 }
233 w[j] = sw;
234 for (k=0;k<m;k++) u(k,j) = su[k];
235 for (k=0;k<n;k++) v(k,j) = sv[k];
236
237 }
238 } while (inc > 1);
239 for (k=0;k<n;k++) {
240 s=0;
241 for (i=0;i<m;i++) if (u(i,k) < 0.) s++;
242 for (j=0;j<n;j++) if (v(j,k) < 0.) s++;
243 if (s > (m+n)/2) {
244 for (i=0;i<m;i++) u(i,k) = -u(i,k);
245 for (j=0;j<n;j++) v(j,k) = -v(j,k);
246 }
247 }
248 }
249
250 T tsh = (T)(0.5*sqrt(m+n+1.)*w[0]*eps);
251 return true;
252
253
254
255 }
256
258template <typename T>
259mat<T> null(const mat<T>& a)
260{
261 T eps =std::numeric_limits<T>::epsilon();
262 mat<T> u,v;
263 diag_mat<T> s;
264 svd(a,u,s,v);
265
266 unsigned r = 0;
267 T tol;
268 if(a.nrows() > a.ncols())
269 tol = (T)a.nrows()*eps;
270 else
271 tol = (T)a.ncols()*eps;
272
273 for(unsigned i = 0; i < s.ncols(); i++)
274 {
275 if(s(i) > tol)
276 r++;
277 }
278 if (r < a.ncols())
279 {
280
281 mat<T> N = v.sub_mat(0, r,v.nrows(),a.ncols()-r);
282 for(unsigned i = 0; i < N.nrows();i++)
283 for(unsigned j = 0; j < N.ncols();j++)
284 if(std::abs(N(i,j)) < eps) N(i,j)=(T)0;
285
286 return N;
287 }
288 else
289 return zeros<T> (a.ncols(), 0);
290
291}
292
293template <typename T>
294mat<T> pseudo_inv(const mat<T>& a, T eps = std::numeric_limits<T>::epsilon())
295 {
296 mat<T> u,v;
297 diag_mat<T> s;
298 svd(a,u,s,v);
299 for(unsigned i = 0; i < s.ncols(); i++)
300 {
301 if(s(i) > eps)
302 s(i)=1/s(i);
303 else
304 s(i) = 0;
305 }
306 return v*s*transpose(u);
307
308 }
309
310
312template <typename T>
313mat<T> null(const mat<T>& a, T tol)
314{
315 T eps =std::numeric_limits<T>::epsilon();
316 mat<T> u,v;
317 diag_mat<T> s;
318 svd(a,u,s,v);
319 unsigned r = 0;
320
321
322 for(unsigned i = 0; i < s.ncols(); i++)
323 {
324 if(s(i) > tol)
325 r++;
326 }
327 if (r < a.ncols())
328 {
329
330 mat<T> N = v.submat(0, r,v.nrows(),a.ncols()-r);
331 for(unsigned i = 0; i < N.nrows();i++)
332 for(unsigned j = 0; j < N.ncols();j++)
333 if(std::abs(N(i,j)) < eps) N(i,j)=(T)0;
334
335 return N;
336 }
337 else
338 return zeros<T> (a.ncols(), 0);
339
340}
341
342
344template<typename T>
345unsigned rank(const mat<T>& a)
346{
347 mat<T> u,v;
348 diag_mat<T> s;
349 svd(a,u,s,v);
350 unsigned r = 0;
351 T tol;
352 if(a.nrows() > a.ncols())
353 tol = (T)a.nrows()*std::numeric_limits<T>::epsilon();
354 else
355 tol = (T)a.ncols()*std::numeric_limits<T>::epsilon();
356
357 for(unsigned i = 0; i < s.ncols(); i++)
358 {
359 if(s(i) > tol)
360 r++;
361 }
362 return r;
363}
364
366template<typename T>
367unsigned rank(const mat<T>& a, T tol)
368{
369 mat<T> u,v;
370 diag_mat<T> s;
371 svd(a,u,s,v);
372 unsigned rank = 0;
373
374 for(unsigned i = 0; i < s.ncols(); i++)
375 {
376 if(s(i) > tol)
377 rank++;
378 }
379 return rank;
380}
381
382
385template <typename T>
386unsigned solve_underdetermined_system(const cgv::math::mat<T>& A, const cgv::math::vec<T>& b,
387 cgv::math::vec<T>& p, cgv::math::mat<T>& N)
388{
389 T eps =std::numeric_limits<T>::epsilon();
390 mat<T> u,v;
391 diag_mat<T> s;
392 svd(A,u,s,v);
393 //compute rank
394 unsigned r = 0;
395 T tol;
396 if(A.nrows() > A.ncols())
397 tol = (T)A.nrows()*eps;
398 else
399 tol = (T)A.ncols()*eps;
400
401 for(unsigned i = 0; i < s.ncols(); i++)
402 {
403 if(s(i) > tol)
404 r++;
405 else
406 s(i)=0;
407 }
408 p=v*s*transpose(u)*b;
409 if (r < A.ncols())
410 {
411
412 N = v.sub_mat(0, r,v.nrows(),A.ncols()-r);
413 for(unsigned i = 0; i < N.nrows();i++)
414 for(unsigned j = 0; j < N.ncols();j++)
415 if(std::abs(N(i,j)) < eps) N(i,j)=(T)0;
416
417 return r;
418 }
419 else
420 {
421 N = zeros<T> (A.ncols(), 0);
422 return r;
423 }
424}
425
426
427
428
429
430
431
432
433}
434
435}
cgv::math::mat
A matrix type (full column major storage) The matrix can be loaded directly into OpenGL without need ...
Definition mat.h:208
cgv::math::mat::ncols
unsigned ncols() const
number of columns
Definition mat.h:546
cgv::math::mat::sub_mat
mat< T > sub_mat(unsigned top, unsigned left, unsigned rows, unsigned cols) const
create submatrix m(top,left)...m(top+rows,left+cols)
Definition mat.h:797
cgv::math::mat::nrows
unsigned nrows() const
number of rows
Definition mat.h:540
cgv::math::vec
A column vector class.
Definition vec.h:28
cgv
the cgv namespace
Definition print.h:11