postfish/reconstruct.c

   1 /*
   2  *
   3  *  postfish
   4  *
   5  *      Copyright (C) 2002-2005 Monty and Xiph.Org
   6  *
   7  *  Postfish is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License as published by
   9  *  the Free Software Foundation; either version 2, or (at your option)
  10  *  any later version.
  11  *
  12  *  Postfish is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU General Public License
  18  *  along with Postfish; see the file COPYING.  If not, write to the
  19  *  Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  20  *
  21  *
  22  */
  23
  24 /* arbitrary reconstruction filter.  Postfish uses this for declipping.
  25
  26    Many thanks to Johnathan Richard Shewchuk and his excellent paper
  27    'An Introduction to the Conjugate Gradient Method Without the
  28    Agonizing Pain' for the additional understanding needed to make the
  29    n^3 -> n^2 log n jump possible. Google for it, you'll find it. */
  30
  31 #include <string.h>
  32 #include <fftw3.h>
  33 #include <math.h>
  34 #include "reconstruct.h"
  35
  36 /* fftw3 requires this kind of static setup */
  37 static fftwf_plan fftwf_qf;
  38 static fftwf_plan fftwf_qb;
  39 static fftwf_plan fftwf_sf;
  40 static fftwf_plan fftwf_sb;
  41 static float *q;
  42 static float *s;
  43 static int blocksize=0;
  44
  45 void reconstruct_init(int minblock,int maxblock){
  46   int i;
  47
  48   q=fftwf_malloc((maxblock+2)*sizeof(*q));
  49   s=fftwf_malloc((maxblock+2)*sizeof(*s));
  50
  51   /* fftw priming trick; run it thorugh the paces and prime a plan for
  52      every size we may need.  fftw will cache the information and not
  53      need to re-measure later */
  54
  55   for(i=minblock;i<=maxblock;i<<=1){
  56     fftwf_qf=fftwf_plan_dft_r2c_1d(i,q,(fftwf_complex *)q,FFTW_MEASURE);
  57     fftwf_qb=fftwf_plan_dft_c2r_1d(i,(fftwf_complex *)q,q,FFTW_MEASURE);
  58     fftwf_destroy_plan(fftwf_qf);
  59     fftwf_destroy_plan(fftwf_qb);
  60   }
  61 }
  62
  63
  64 void reconstruct_reinit(int n){
  65   if(blocksize!=n){
  66     if(blocksize){
  67       fftwf_destroy_plan(fftwf_qf);
  68       fftwf_destroy_plan(fftwf_qb);
  69       fftwf_destroy_plan(fftwf_sf);
  70       fftwf_destroy_plan(fftwf_sb);
  71       fftwf_free(q);
  72       fftwf_free(s);
  73     }
  74     blocksize=n;
  75
  76     q=fftwf_malloc((n+2)*sizeof(*q));
  77     s=fftwf_malloc((n+2)*sizeof(*s));
  78     fftwf_qf=fftwf_plan_dft_r2c_1d(n,q,(fftwf_complex *)q,FFTW_MEASURE);
  79     fftwf_qb=fftwf_plan_dft_c2r_1d(n,(fftwf_complex *)q,q,FFTW_MEASURE);
  80     fftwf_sf=fftwf_plan_dft_r2c_1d(n,s,(fftwf_complex *)s,FFTW_MEASURE);
  81     fftwf_sb=fftwf_plan_dft_c2r_1d(n,(fftwf_complex *)s,s,FFTW_MEASURE);
  82   }
  83 }
  84
  85 static void AtWA(float *w,int n){
  86   int i;
  87   fftwf_execute(fftwf_qf);
  88   for(i=0;i<n+2;i++)q[i]*=w[i];
  89   fftwf_execute(fftwf_qb); /* this is almost the same as A'; see the
  90                               correction factor rolled into w at the
  91                               beginning of reconstruct() */
  92 }
  93
  94 /* This is not the inverse of XA'WAX; the algebra isn't valid (due to
  95    the singularity of the selection matrix X) and as such is useless
  96    for direct solution.  However, it does _approximate_ the inverse
  97    and as such makes an excellent system preconditioner. */
  98 static void precondition(float *w,int n){
  99   int i;
 100
 101   /* no need to remove scaling of result; the relative stretching of
 102      the solution space is what's important */
 103
 104   fftwf_execute(fftwf_sf); /* almost the same as A^-1'; see the correction
 105                               factor rolled into w at the beginning of
 106                               reconstruct() */
 107   for(i=0;i<n+2;i++)s[i]/=w[i];
 108   fftwf_execute(fftwf_sb);
 109 }
 110
 111 static float inner_product(float *a, float *b, int n){
 112   int i;
 113   float acc=0.;
 114   for(i=0;i<n;i++)acc+=a[i]*b[i];
 115   return acc;
 116 }
 117
 118 void reconstruct(float *x, float *w,
 119                  float *flag, float e,int max){
 120   int n=blocksize;
 121   int i,j;
 122   float Atb[n];
 123   float r[n];
 124   float d[n];
 125   float phi_new,phi_old,res_0,res_new;
 126   float alpha,beta;
 127
 128   /* hack; roll a correction factor for A'/A-1 into w */
 129   for(j=1;j<n-1;j++)w[j]*=.5;
 130
 131   /* compute initial Atb */
 132   for(j=0;j<n;j++)q[j]=x[j]*(flag[j]-1.);
 133   AtWA(w,n);
 134   res_new=res_0=inner_product(q,q,n);
 135   for(j=0;j<n;j++)Atb[j]=q[j];
 136
 137   /* compute initial residue */
 138   for(j=0;j<n;j++)q[j]=x[j]*flag[j];
 139   AtWA(w,n);
 140   for(j=0;j<n;j++)s[j]=r[j]=(Atb[j]-q[j])*flag[j];
 141
 142   /* initial preconditioning */
 143   precondition(w,n);
 144   for(j=0;j<n;j++)q[j]=d[j]=s[j]*=flag[j];
 145
 146   phi_new=inner_product(r,d,n);
 147
 148   for(i=0;i<max && sqrt(res_new)/sqrt(res_0)>e;i++){
 149
 150     AtWA(w,n);
 151     alpha=phi_new/inner_product(d,q,n);
 152     for(j=0;j<n;j++)x[j]+=alpha*d[j];
 153
 154     if((i & 0x3f)==0x3f){
 155       for(j=0;j<n;j++)q[j]=x[j]*flag[j];
 156       AtWA(w,n);
 157       for(j=0;j<n;j++)r[j]=(Atb[j]-q[j])*flag[j];
 158     }else
 159       for(j=0;j<n;j++)r[j]-=alpha*q[j]*flag[j];
 160
 161     /* apply preconditioner */
 162     for(j=0;j<n;j++)s[j]=r[j]*flag[j];
 163     precondition(w,n);
 164     for(j=0;j<n;j++)s[j]*=flag[j];
 165
 166     phi_old=phi_new;
 167     phi_new=inner_product(r,s,n);
 168     res_new=inner_product(r,r,n);
 169     beta=phi_new/phi_old;
 170     for(j=0;j<n;j++) q[j]=d[j]=s[j]+beta*d[j];
 171
 172   }
 173
 174 }