1 #include "mrilib.h"
2
3 typedef struct {
4 int ndim ;
5 float * cmat , * cfac , * mvec ;
6 } covmat ;
7
8 #define CM(i,j) cmat[(i)+(j)*ndim]
9 #define CH(i,j) cfac[(i)+(j)*ndim]
10
11 /*-----------------------------------------------------------------*/
12
forward_solve_inplace(covmat * cv,float * vec)13 void forward_solve_inplace( covmat * cv , float * vec )
14 {
15 register int ndim=cv->ndim , ii,jj ;
16 register float * cfac=cv->cfac , sum ;
17
18 for( ii=0 ; ii < ndim ; ii++ ){
19 sum = vec[ii] ;
20 for( jj=0 ; jj < ii ; jj++ ) sum -= CH(ii,jj) * vec[jj] ;
21 vec[ii] = sum / CH(ii,ii) ;
22 }
23 return ;
24 }
25
26 /*-----------------------------------------------------------------*/
27
backward_solve_inplace(covmat * cv,float * vec)28 void backward_solve_inplace( covmat * cv , float * vec )
29 {
30 register int ndim=cv->ndim , ii,jj ;
31 register float * cfac=cv->cfac , sum ;
32
33 for( ii=ndim-1 ; ii >= 0 ; ii-- ){
34 sum = vec[ii] ;
35 for( jj=ii+1 ; jj < ndim ; jj++ ) sum -= CH(jj,ii) * vec[jj] ;
36 vec[ii] = sum / CH(ii,ii) ;
37 }
38 return ;
39 }
40
41 /*-----------------------------------------------------------------*/
42
compute_choleski(covmat * cv)43 void compute_choleski( covmat * cv )
44 {
45 register int ndim=cv->ndim , ii,jj,kk ;
46 register float * cmat=cv->cmat , * cfac , sum ;
47
48 if( ndim < 2 || cmat == NULL ) return ;
49
50 if( cv->cfac == NULL )
51 cv->cfac = (float *) malloc(sizeof(float)*ndim*ndim) ;
52
53 cfac = cv->cfac ;
54
55 for( ii=0 ; ii < ndim ; ii++ ){
56 for( jj=0 ; jj < ii ; jj++ ){
57 sum = CM(ii,jj) ;
58 for( kk=0 ; kk < jj ; kk++ ) sum -= CH(ii,kk) * CH(jj,kk) ;
59 CH(ii,jj) = sum / CH(jj,jj) ;
60 }
61 sum = CM(ii,ii) ;
62 for( kk=0 ; kk < ii ; kk++ ) sum -= CH(ii,kk) * CH(ii,kk) ;
63 if( sum <= 0.0 ){ free(cv->cfac); cv->cfac = NULL; return; }
64 CH(ii,ii) = sqrt(sum) ;
65 for( jj=ii+1 ; jj < ndim ; jj++ ) CH(ii,jj) = 0.0 ;
66 }
67 return ;
68 }
69
70 /*-----------------------------------------------------------------*/
71
72 #define CCUT 3.5
73 #define EPS 1.e-4
74
robust_covar(int ndim,int nvec,float ** vec)75 covmat * robust_covar( int ndim , int nvec , float ** vec )
76 {
77 covmat * cv ;
78 float *nmat, *cmat , fnvec,fndim,cnorm,csum , *tv , *vv , *mv , *wv ;
79 int ii , jj , kk , nite ;
80 float bcut , cwt ;
81
82 fprintf(stderr,"Enter robust_covar: ndim=%d nvec=%d\n",ndim,nvec) ;
83
84 if( ndim < 2 || nvec < ndim || vec == NULL ) return NULL ;
85
86 cv = (covmat *) malloc(sizeof(covmat)) ;
87 cv->ndim = ndim ;
88 cv->cmat = NULL ;
89 cv->cfac = NULL ;
90 cv->mvec = NULL ;
91
92 nmat = (float *) malloc(sizeof(float)*ndim*ndim) ; /* matrix */
93 tv = (float *) malloc(sizeof(float)*ndim) ; /* temp vector */
94 mv = (float *) malloc(sizeof(float)*ndim) ; /* mean vector */
95 wv = (float *) malloc(sizeof(float)*nvec) ; /* weight vector */
96
97 fnvec = 1.0/nvec ; fndim = 1.0/ndim ;
98 bcut = 1.0 + CCUT*sqrt(fndim) ;
99
100 /* compute initial mean & covariance matrix with all weights = 1 */
101
102 for( jj=0 ; jj < ndim ; jj++ ) mv[jj] = 0.0 ;
103
104 for( kk=0 ; kk < nvec ; kk++ ){ /* mean vector sum */
105 vv = vec[kk] ;
106 for( jj=0 ; jj < ndim ; jj++ ) mv[jj] += vv[jj] ;
107 }
108 for( jj=0 ; jj < ndim ; jj++ ) mv[jj] *= fnvec ; /* scale mean vector */
109
110 for( jj=0 ; jj < ndim ; jj++ )
111 for( ii=0 ; ii < ndim ; ii++ ) nmat[ii+jj*ndim] = 0.0 ;
112
113 for( kk=0 ; kk < nvec ; kk++ ){ /* covariance matrix sum */
114 vv = vec[kk] ;
115 for( jj=0 ; jj < ndim ; jj++ ){
116 for( ii=0 ; ii <= jj ; ii++ )
117 nmat[ii+jj*ndim] += (vv[ii]-mv[ii])*(vv[jj]-mv[jj]) ;
118 }
119 }
120 for( jj=0 ; jj < ndim ; jj++ ){ /* scale covariance matrix */
121 for( ii=0 ; ii < jj ; ii++ )
122 nmat[jj+ii*ndim] = (nmat[ii+jj*ndim] *= fnvec) ;
123 nmat[jj+jj*ndim] *= fnvec ;
124 }
125
126 /* now iterate */
127
128 nite = 0 ;
129
130 while(1){
131
132 nite++ ;
133
134 cmat = cv->cmat = nmat ; /* put old matrix into cv */
135 compute_choleski(cv) ; /* decompose it */
136
137 nmat = (float *) malloc(sizeof(float)*ndim*ndim) ; /* new matrix */
138 cv->mvec = mv ; /* old mean vector */
139 mv = (float *) malloc(sizeof(float)*ndim) ; /* new mean vector */
140
141 for( jj=0 ; jj < ndim ; jj++ ){ /* initialize new things to zero */
142 mv[jj] = 0.0 ;
143 for( ii=0 ; ii < ndim ; ii++ ) nmat[ii+jj*ndim] = 0.0 ;
144 }
145
146 fprintf(stderr,"\niteration %2d:\n",nite) ;
147
148 /* update mean */
149
150 csum = 0.0 ;
151 for( kk=0 ; kk < nvec ; kk++ ){
152 vv = vec[kk] ;
153
154 /* -1/2 */
155 /* compute tv = [cmat] (vv-mvec) */
156
157 for( jj=0 ; jj < ndim ; jj++ ) tv[jj] = vv[jj] - cv->mvec[jj] ;
158 forward_solve_inplace(cv,tv) ;
159
160 /* compute norm of tv, then weighting factor for this vector */
161
162 cnorm = 0.0 ; for( ii=0 ; ii < ndim ; ii++ ) cnorm += tv[ii]*tv[ii] ;
163 cnorm = cnorm*fndim ;
164 cnorm = (cnorm <= bcut) ? 1.0 : bcut/cnorm ;
165 wv[kk] = cnorm ; csum += cnorm ;
166 #if 0
167 fprintf(stderr," weight %2d = %12.4g\n",kk,cnorm) ;
168 #endif
169 #if 0
170 for( jj=0 ; jj < ndim ; jj++ ) fprintf(stderr," %f:%f:%f",vv[jj],tv[jj],cv->mvec[jj]) ;
171 fprintf(stderr,"\n") ;
172 #endif
173
174 /* add vv into accumulating mean, with weight cnorm */
175
176 for( jj=0 ; jj < ndim ; jj++ ) mv[jj] += cnorm*vv[jj] ;
177 }
178 #if 0
179 fprintf(stderr," wv:");for(kk=0;kk<nvec;kk++)fprintf(stderr," %11.3g",wv[kk]);fprintf(stderr,"\n csum: %11.3g\n",csum);
180 #endif
181 csum = 1.0 / csum ; cwt = nvec*csum ;
182 for( jj=0 ; jj < ndim ; jj++ ) mv[jj] *= csum ; /* scale new mean */
183
184 /* update covariance */
185
186 for( kk=0 ; kk < nvec ; kk++ ){
187 vv = vec[kk] ; cnorm = wv[kk] ;
188 for( jj=0 ; jj < ndim ; jj++ ){
189 for( ii=0 ; ii <= jj ; ii++ )
190 nmat[ii+jj*ndim] +=
191 cnorm*(vv[ii]-cv->mvec[ii])*(vv[jj]-cv->mvec[jj]) ;
192 }
193 }
194 #define DDD csum
195 for( jj=0 ; jj < ndim ; jj++ ){
196 for( ii=0 ; ii < jj ; ii++ )
197 nmat[jj+ii*ndim] = (nmat[ii+jj*ndim] *= DDD) ;
198 nmat[jj+jj*ndim] *= DDD ;
199 }
200
201 /* check for convergence - L1 norm */
202
203 cnorm = csum = 0.0 ;
204 for( jj=0 ; jj < ndim ; jj++ ){
205 for( ii=0 ; ii <= jj ; ii++ ){
206 cnorm += fabs( nmat[ii+jj*ndim] - cmat[ii+jj*ndim] ) ;
207 csum += fabs( nmat[ii+jj*ndim] ) ;
208 }
209 }
210
211 fprintf(stderr," |dif|=%12.4g |mat|=%12.4g cwt=%12.4g\n",cnorm,csum,cwt) ;
212 fprintf(stderr," matrix:\n") ;
213 for( ii=0 ; ii < ndim ; ii++ ){
214 fprintf(stderr," Row%2d: %12.4g ",ii,mv[ii]) ;
215 for( jj=0 ; jj < ndim ; jj++ ) fprintf(stderr," %12.4g",nmat[ii+jj*ndim]) ;
216 fprintf(stderr,"\n") ;
217 }
218
219 free(cv->cmat) ; free(cv->mvec) ;
220 if( cnorm <= EPS*csum ){ cv->cmat = nmat; cv->mvec = mv; break; } /* exit loop */
221 }
222
223 free(wv) ; free(tv) ; compute_choleski(cv) ; return cv ;
224 }
225
226 /*----------------------------------------------------------------------------*/
227
main(int argc,char * argv[])228 int main( int argc , char * argv[] )
229 {
230 MRI_IMAGE * im , * qim ;
231 float * far , ** vec ;
232 int ii,jj , ndim,nvec ;
233
234 if( argc < 2 ){printf("Usage: rcov imfile\n"); exit(0); }
235
236 im = mri_read_just_one( argv[1] ) ;
237 if( im == NULL ) exit(1) ;
238 if( im->kind != MRI_float ){
239 qim = mri_to_float(im) ; mri_free(im) ; im = qim ;
240 }
241 qim = mri_transpose(im) ; mri_free(im) ; im = qim ;
242
243 ndim = im->nx ; nvec = im->ny ;
244 far = MRI_FLOAT_PTR(im) ;
245 vec = (float **) malloc(sizeof(float *)*nvec) ;
246 for( jj=0 ; jj < nvec ; jj++ ) vec[jj] = far + jj*ndim ;
247
248 #if 0
249 far = (float *) malloc(sizeof(float)*ndim) ;
250 for( ii=0 ; ii < ndim ; ii++ ) far[ii] = 0.0 ;
251 for( jj=0 ; jj < nvec ; jj++ )
252 for( ii=0 ; ii < ndim ; ii++ ) far[ii] += vec[jj][ii] ;
253 for( ii=0 ; ii < ndim ; ii++ ) far[ii] /= nvec ;
254 for( jj=0 ; jj < nvec ; jj++ )
255 for( ii=0 ; ii < ndim ; ii++ ) vec[jj][ii] -= far[ii] ;
256 #endif
257
258 (void) robust_covar( ndim , nvec , vec ) ;
259 exit(0) ;
260 }
261