00001 #include <math.h>
00002 #include <stdlib.h>
00003 #include <stdio.h>
00004 #include <math.h>
00005 #include <string.h>
00006 #include <ctype.h>
00007 #include <float.h>
00008
00009 #include "lapackblas.h"
00010 #include "utilnum.h"
00011
00012 int asta2(float *img, int nx, int ny, int ri, double *abaloc, int *klploc)
00013 {
00014
00015
00016
00017
00018
00019 int xcent = (nx / 2) + 1;
00020 int ycent = (ny / 2) + 1;
00021 int r_squared = ri*ri;
00022
00023 int x_summand, y_summand;
00024
00025 for ( int i = 0 ; i < nx ; ++i ) {
00026 x_summand = (i-xcent) * (i-xcent);
00027 for ( int j = 0 ; j < ny ; ++j ) {
00028 y_summand = (j-ycent) * (j-ycent);
00029 if ( x_summand + y_summand > r_squared ) {
00030 *abaloc += (double) img[j*nx + i];
00031
00032 img[j*nx+i]=0.0;
00033 ++*klploc;
00034 }
00035 }
00036 }
00037
00038 return 0;
00039 }
00040
00041 int ifix(float a)
00042 {
00043 int ia = 0;
00044 if (a >= 0.0) {
00045 ia = (int)floor(a);
00046 }
00047 else {
00048 ia = (int)ceil(a);
00049 }
00050 return ia;
00051 }
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 void svd(float **A, int m, int n, float **U, float *s, float **V)
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 {
00083 char jobu = 'A';
00084 char jobv = 'A';
00085
00086 int i;
00087
00088
00089
00090 integer lwork = 100;
00091 integer lda=m;
00092 integer ldu=m;
00093 integer ldv=n;
00094 integer info;
00095 integer mm = m;
00096 integer nn = n;
00097
00098 float ** A_column = matrix(n,m);
00099 float * work;
00100
00101 work = myvector((int) lwork);
00102
00103
00104
00105 column_orient(A, m, n, A_column);
00106
00107
00108
00109
00110
00111
00112
00113
00114 sgesvd_(&jobu, &jobv, &mm, &nn, *A_column, &lda, s, *U, &ldu, *V, &ldv,
00115 work, &lwork, &info);
00116
00117
00118
00119
00120
00121 transpose_matrix(U, m, m);
00122
00123 free_vector(work);
00124 free_matrix(A_column);
00125
00126
00127 }
00128
00129
00130 void tsvd(float **U, float *s, float **V, float *b, int m, int n, float omega, float *x, float *tol)
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 {
00149 int i, k;
00150 float * sinv = myvector(n);
00151 float * bhat = myvector(m);
00152
00153 matvec_multT(U, b, m, m, bhat);
00154
00155
00156
00157
00158
00159 k = 0;
00160 if (*tol < (float) 0.0)
00161 gcv_tsvd(s, bhat, m, n, omega, tol);
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173 for (i=0; i<n; i++){
00174 if (fabs(s[i]) >= *tol){
00175 sinv[i] = (float) 1.0 / s[i];
00176 k++;
00177 }
00178 }
00179 *tol = k;
00180
00181
00182
00183
00184
00185
00186 for (i=0; i<n; i++)
00187 bhat[i] = sinv[i]*bhat[i];
00188
00189 matvec_mult(V, bhat, n, n, x);
00190
00191
00192 free_vector(sinv);
00193 free_vector(bhat);
00194 }
00195
00196
00197
00198
00199
00200 void gcv_tsvd(float *s, float *bhat, int m, int n, float omega, float * tol)
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 {
00213 float * bhat_copy = myvector(m);
00214 float * rho = myvector(m-1);
00215 float * G = myvector(m-1);
00216 int i, kk=0;
00217 float ni;
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230 copy_vector(bhat, bhat_copy, m);
00231 for (i=0; i<m; i++)
00232 bhat_copy[i] = fabs( bhat[i] );
00233
00234 rho[m-2] = bhat_copy[m-1]*bhat_copy[m-1];
00235
00236
00237
00238
00239 for (i = m-3; i>-1; i--){
00240 rho[i] = rho[i+1] + bhat_copy[i+1]*bhat_copy[i+1];
00241 }
00242
00243
00244 for(i=0; i< m-1; i++){
00245 ni = (float) m - omega*(i+1);
00246 G[i] = (float) n* rho[i]/(ni*ni);
00247 }
00248
00249
00250
00251
00252 for (i=0; i < m-2; i++){
00253 if (G[i+1]<G[i])
00254 kk = i+1;
00255 }
00256
00257 *tol = s[kk];
00258
00259
00260 free_vector(bhat_copy);
00261 free_vector(rho);
00262 free_vector(G);
00263 }
00264
00265
00266
00267
00268 void tikhonov(float **U, float *s, float **V, float *b, int m, int n, float omega, float *x, float * alpha)
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287 {
00288 int i, MaxIts = 500;
00289 float * bhat = myvector(m);
00290 float * xhat = myvector(n);
00291 float * D = myvector(n);
00292
00293 matvec_multT(U, b, m, m, bhat);
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 if (*alpha < (float)0.0)
00310 *alpha = GCVmin_Tik(s, bhat, m, n, MaxIts, omega);
00311
00312
00313
00314
00315 for (i=0; i<n;i++){
00316 D[i] = fabs(s[i])*fabs(s[i]) + (*alpha)*(*alpha);
00317 xhat[i] = (s[i] * bhat[i])/D[i];
00318 }
00319
00320 matvec_mult(V, xhat, n, n, x);
00321
00322 free_vector(bhat);
00323 free_vector(xhat);
00324 free_vector(D);
00325 }
00326
00327
00328
00329 float GCVmin_Tik(float *s, float *bhat, int m, int n, int MaxIts, float omega)
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343 {
00344 float ax, bx, seps, c, a, b, v, w, sf, d, e, x, fx, fv, fw, xm, tol, tol1, tol2, r, q, p, si, fu, fval, xf;
00345 int funccount, iter, maxfun, gs;
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359 ax = (float) 0.0;
00360
00361 bx = s[0];
00362
00363
00364 seps = sqrt(FLT_EPSILON);
00365 c = (float)0.5*((float)3.0 - sqrt((float)5.0));
00366 a = ax;
00367 b = bx;
00368 v = a + c*(b-a);
00369 w = v;
00370 xf = v;
00371 d = (float)0.0;
00372 e = (float)0.0;
00373 x = xf;
00374
00375 fx = GCVfun_Tik(x, s, bhat, m,n, omega);
00376
00377 fv = fx;
00378 fw = fx;
00379 xm = (float)0.5*(a+b);
00380 tol = (float).0001;
00381 tol1 = seps*fabs(xf) + tol/(float)3.0;
00382 tol2 = (float)2.0*tol1;
00383 funccount = 0;
00384 iter = 0;
00385 maxfun = 500;
00386
00387 for (iter = 1; iter< MaxIts; iter++){
00388 gs = 1;
00389
00390 if (fabs(e) > tol1){
00391 gs = 0;
00392 r = (xf-w)*(fx-fv);
00393 q = (xf-v)*(fx-fw);
00394 p = (xf-v)*q-(xf-w)*r;
00395 q = (float)2.0*(q-r);
00396 if (q > (float)0.0)
00397 p = -p;
00398 q = fabs(q);
00399 r = e;
00400 e = d;
00401
00402 if ( (fabs(p)<fabs((float)0.5*q*r)) && (p>q*(a-xf)) && (p<q*(b-xf)) ){
00403
00404 d = p/q;
00405 x = xf+d;
00406 if (((x-a) < tol2) || ((b-x) < tol2)){
00407 if(xm-xf <0)
00408 si = -(float)1.0;
00409 else
00410 si = (float)1.0;
00411
00412 d = tol1*si;
00413 }
00414 }
00415 else{
00416
00417 gs = 1;
00418 }
00419 }
00420 if (gs){
00421
00422 if (xf >= xm)
00423 e = a-xf;
00424 else
00425 e = b-xf;
00426
00427 d = c*e;
00428 }
00429
00430 if(d <0)
00431 si = -(float)1.0;
00432 else
00433 si = (float)1.0;
00434
00435 x = xf + si * max( fabs(d), tol1 );
00436 fu = GCVfun_Tik( x, s, bhat, m,n, omega);
00437 funccount = funccount + 1;
00438 if (fu <= fx){
00439 if (x >= xf)
00440 a = xf;
00441 else
00442 b = xf;
00443
00444 v = w;
00445 fv = fw;
00446 w = xf;
00447 fw = fx;
00448 xf = x;
00449 fx = fu;
00450 }
00451 else{
00452 if (x < xf)
00453 a = x;
00454 else
00455 b = x;
00456
00457 if ( (fu <= fw) || (w == xf) ){
00458 v = w;
00459 fv = fw;
00460 w = x;
00461 fw = fu;
00462 }
00463 else if ((fu <= fv) || (v == xf) || (v == w)){
00464 v = x;
00465 fv = fu;
00466 }
00467 }
00468 xm = (float)0.5*(a+b);
00469 tol1 = seps*fabs(xf) + tol/(float)3.0;
00470 tol2 = (float)2.0*tol1;
00471 if (funccount >= maxfun || fabs(xf-xm) <= (tol2 - (float)0.5*(b-a))){
00472 fval = fx;
00473 return xf;
00474 }
00475 }
00476 fval = fx;
00477 return xf;
00478 }
00479
00480
00481
00482
00483 float GCVfun_Tik(float alpha, float * s, float * bhat, int m, int n, float omega)
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495 {
00496 float t0, G;
00497 float * hold = myvector(m-n);
00498 float * tt= myvector(n);
00499 float * t1= myvector(n);
00500 float * t2= myvector(n);
00501 float * t3= myvector(n);
00502 int i;
00503
00504 for (i=0; i< m-n; i++)
00505 hold[i] = fabs(bhat[n])*fabs(bhat[n]);
00506 t0 = sum_vector(hold,m-n);
00507
00508 for (i = 0; i<n; i++){
00509 tt[i] = (float)1.0 /(s[i]*s[i] +alpha*alpha);
00510 t1[i] = alpha*alpha * tt[i];
00511 t2[i] = fabs(bhat[i] * t1[i]) * fabs(bhat[i] * t1[i]);
00512 t3[i] = t1[i] + ((float)1.0 - omega)* (s[i]*s[i]) * tt[i];
00513 }
00514 G = (float) n*(sum_vector(t2,n) + t0) / ((sum_vector(t3,n) + (float)m - (float)n)* (sum_vector(t3,n) + (float)m - (float)n));
00515
00516
00517
00518 free_vector(hold);
00519 free_vector(tt);
00520 free_vector(t1);
00521 free_vector(t2);
00522 free_vector(t3);
00523
00524 return G;
00525 }
00526
00527
00528
00529 void generate_noise(float *E, int m, int n)
00530
00531
00532
00533
00534
00535
00536
00537
00538 {
00539 float **U = matrix(m,12);
00540 float nm, a = -6;
00541 int i, sz = 1, j;
00542 float *o = myvector(m);
00543
00544 fill_ones(o,m);
00545
00546 fill_rand(U, m, 12);
00547 row_sum(U, m, 12, E);
00548
00549 for(j=0; j<m; j++)
00550 E[j] = E[j] + a*o[j];
00551
00552
00553
00554 nm = norm(E, m);
00555
00556 for(i = 0; i < m; i++)
00557 E[i] = E[i] / nm;
00558
00559 free_matrix(U);
00560 free_vector(o);
00561
00562 }
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606 float **matrix(int m, int n)
00607 {
00608 int i;
00609 float **A=(float **) calloc(m, sizeof(float*));
00610 if (A == NULL)
00611 {
00612 fprintf(stderr, "Allocation failure in matrix()");
00613 exit (1);
00614 }
00615 A[0]=(float *) calloc(m * n, sizeof(float));
00616 if (A[0] == NULL)
00617 {
00618 fprintf(stderr, "Allocation failure in matrix()");
00619 exit (1);
00620 }
00621 for(i=1; i < m; i++)
00622 A[i]=A[0] + i * n;
00623 return A;
00624 }
00625
00626
00627 float **submatrix(float **A, int m, int n, int newm, int newn)
00628 {
00629 int i,j;
00630 float **B = matrix(newm, newn);
00631 if (newm > m || newn > n){
00632 printf("error in submatrix\n");
00633
00634 return B;
00635 }
00636 for(i=0;i<newm;i++)
00637 for(j=0;j<newn;j++)
00638 B[i][j]=A[i][j];
00639
00640 return B;
00641 }
00642
00643
00644 void free_matrix(float **A)
00645 {
00646 free(A[0]);
00647 free(A);
00648 }
00649
00650
00651 void fill_rand(float **A, int m, int n)
00652 {
00653 int i,j;
00654 for(i = 0; i < m; i++) {
00655 for (j = 0; j < n; j++)
00656 A[i][j]=frand();
00657
00658 }
00659 }
00660
00661
00662 void print_matrix(float **A, int m, int n)
00663 {
00664 int i,j;
00665 for (i=0; i< m; i++)
00666 {
00667 for (j=0; j< n; j++)
00668 printf ("%.15f ", A[i][j]);
00669 putchar ('\n');
00670 }
00671 }
00672
00673
00674 void transpose_matrix(float **A, int m, int n)
00675 {
00676 float save;
00677 int i,j;
00678 for (i = 0; i < m; i++){
00679 for (j = i+1; j < n; j++)
00680 {
00681 save = A[i][j];
00682 A[i][j] = A[j][i];
00683 A[j][i] = save;
00684 }
00685 }
00686 }
00687
00688
00689
00690 void column_orient(float **A, int m, int n, float ** B)
00691 {
00692 int i,j;
00693 for (j = 0; j < n; j++){
00694 for (i = 0; i < m; i++)
00695 {
00696 B[j][i] = A[i][j];
00697 }
00698 }
00699 }
00700
00701
00702 void row_sum(float **A, int m, int n, float *e)
00703 {
00704 int i,j;
00705 for (i = 0; i < m; i++){
00706 for (j = 0; j < n; j++)
00707 {
00708 e[i] = e[i]+ A[i][j];
00709 }
00710 }
00711 }
00712
00713
00714 void copy_matrix(float **A, float **B, int m, int n)
00715 {
00716 int i,j;
00717 for(i = 0; i < m; i++) {
00718 for (j = 0; j < n; j++)
00719 B[i][j]=A[i][j];
00720
00721 }
00722 }
00723
00724
00725
00726
00727
00728 float *myvector(int n)
00729 {
00730 float *x=(float *) calloc(n, sizeof(float));
00731 if (x == NULL)
00732 {
00733 fprintf(stderr, "Allocation failure in matrix()");
00734 exit (1);
00735 }
00736 return x;
00737 }
00738
00739
00740 float *subvector(float *x, int n, int newn)
00741 {
00742 int i;
00743 float *y = myvector(newn);
00744 if (newn > n){
00745 printf("error in subvector");
00746 return y;
00747 }
00748 for(i=0;i<newn;i++)
00749 y[i]=x[i];
00750
00751 return y;
00752 }
00753
00754
00755 void free_vector(float *x)
00756 {
00757 free(x);
00758 }
00759
00760
00761 void print_vector(float *x, int n)
00762 {
00763 int i;
00764 for (i=0; i< n; i++)
00765 {
00766 printf ("%.15f \n ", x[i]);
00767 }
00768 }
00769
00770
00771 void fill_ones(float *x, int m)
00772 {
00773 int i;
00774 for (i = 0; i < m; i++){
00775 x[i] = (float) 1.0;
00776 }
00777 }
00778
00779
00780 float sum_vector(float *x, int n)
00781 {
00782 int i;
00783 float sum = 0.0;
00784 for (i = 0; i < n; i++){
00785 sum = sum + x[i];
00786 }
00787 return sum;
00788 }
00789
00790
00791 void copy_vector(float *x, float *y, int n)
00792 {
00793 int i;
00794 for(i = 0; i < n; i++)
00795 y[i]=x[i];
00796 }
00797
00798
00799 void fillveczeros(float *x, int n)
00800 {
00801 int i;
00802 for (i=0; i<n; i++)
00803 x[i] = 0.0;
00804 }
00805
00806
00807 float norm(float *x, int n)
00808 {
00809 float x_norm = 0.0;
00810 int i;
00811 for (i=0; i<n; i++)
00812 x_norm = x_norm + x[i]*x[i];
00813
00814 x_norm = sqrt(x_norm);
00815
00816
00817
00818 return x_norm;
00819 }
00820
00821
00822 void alphaxpy(float alpha, float *x, float *y, int n)
00823 {
00824 int i;
00825 for (i=0; i<n; i++)
00826 y[i] = alpha*x[i] + y[i];
00827 }
00828
00829
00830
00831
00832 void matvec_mult(float **A, float *x, int m, int n, float *b)
00833 {
00834 int i,j;
00835 for(i=0; i< m; i++){
00836 b[i] = 0.0;
00837 for(j=0; j<n; j++){
00838 b[i] = b[i] + A[i][j]*x[j];
00839 }
00840 }
00841
00842
00843 }
00844
00845
00846 void matvec_multT(float **A, float *x, int m, int n, float *b)
00847 {
00848 int i,j;
00849 for(i=0; i< n; i++){
00850 b[i] = 0.0;
00851 for(j=0; j<m; j++){
00852 b[i] = b[i] + A[j][i]*x[j];
00853 }
00854 }
00855
00856
00857 }
00858
00859
00860
00861
00862 void get_col(float ** A, int m, int k, float * ak)
00863 {
00864 int i;
00865 for (i = 0; i < m; i++)
00866 ak[i] = A[i][k];
00867
00868 }
00869
00870
00871
00872
00873 void get_row(float ** A, int n, int k, float * ak)
00874 {
00875 int i;
00876 for (i = 0; i < n; i++)
00877 ak[i] = A[k][i];
00878
00879 }
00880
00881
00882 void put_col(float ** A, int m, float * v, int k)
00883 {
00884 int i;
00885 for (i = 0; i < m; i++)
00886 A[i][k] = v[i];
00887
00888 }
00889