1 package org.lcsim.fit.circle;
2 /**
3 * A non-iterative circle fit based on the algorithm of V. Karimaki.
4 *
5 * @author Norman A. Graf
6 * @version 1.0
7 */
8 public class CircleFitter
9 {
10 /*
11 **********************************************************************
12 SUBROUTINE CIRCLF(XREF,YREF,XX,YY,WW,NP,MODE,IERROR)
13 * ********** ****** *
14 * *
15 * Non-iterative circle fit (V. Karimaki/1991) *
16 * *
17 * XREF,YREF = reference point coordinates (IN) *
18 * XX,YY = arrays of measured coordinates (IN) *
19 * WW = array of weigths (IN) *
20 * NP = number of points (IN) *
21 * MODE = 1: solve parameters plus error matrix (IN) *
22 * 2: point removal/adding and error matrix *
23 * 3: propagation, parameters and error matrix *
24 * -1,-2,-3: as 1,2,3 but without error matrix *
25 * IERROR = error flag (=0 if fit OK) (OUT) *
26 * *
27 * Fit results in COMMON/CIRCFI/: *
28 * RHO = fitted curvature (OUT) *
29 * PHI = fitted direction (OUT) *
30 * DCA = fitted distance to (XREF,YREF) (OUT) *
31 * CHICIR = chi squared of the fit (OUT) *
32 * XPCA,YPCA = point on circle closest to XREF,YREF (OUT) *
33 * COVRFD(6) = covariance matrix of RHO,PHI,DCA (OUT) *
34 * in lower triangular form *
35 * *
36 * NOTES: 1. Weights should be negative for point removal *
37 * 2. MODE = +-2 possible only if COMMON/CIRCFI/ fully *
38 * restored or if call is subsequent to MODE=+-1 *
39 * 3. MODE = +-3 possible only if COMMON/CIRCFI/ is *
40 * restored with the old parameters RHO,...,COVRFD(6) *
41 *--------------------------------------------------------------------*
42 */
43 private double _xref; // x reference position;
44 private double _yref; // y reference position;
45
46 private double _rho; // curvature
47 private double _phi; // phi angle at (_xref, _yref)
48 private double _dca; // distance of closest approach to (_xref, _yref)
49 private double _chicir; // chi-squared of circle fit
50
51 private double _xpca;
52 private double _ypca;
53 private double [] _covrfd;
54 private double _xx0;
55 private double _yy0;
56 private double S1, S2, S3, S4, S5, S6, S7, S8, S9;
57 /**
58 * Default Constructor sets reference point to (0,0)
59 *
60 */
61 public CircleFitter()
62 {
63 _xref=0.;
64 _yref=0.;
65
66 _covrfd = new double[6];
67 }
68
69 /**
70 * Set the reference point for the fit.
71 * @param xref x position of the reference point
72 * @param yref y position of the reference point
73 */
74 public void setreferenceposition( double xref, double yref)
75 {
76 _xref = xref;
77 _yref = yref;
78 }
79
80 /**
81 * Fit the data points
82 * @param XX array of x positions
83 * @param YY array of y positions
84 * @param WW array of weights
85 * @param NP number of points
86 * @return true if fit is successful
87 */
88 public boolean fit(double[] XX, double[] YY, double[] WW, int NP)
89 {
90 // COMMON /CIRCFI/ RHO,PHI,DCA,CHICIR,XPCA,YPCA,COVRFD(6)
91 // +, XX0,YY0,S1,S2,S3,S4,S5,S6,S7,S8,S9
92
93 double SR,HSR,S1I,XMEAN,YMEAN,RRMEAN,XC,YC,WT,WX,WY,RR,WR;
94 double COV1,COV2,COV3,COV4,COV5,COV6,Y2FI,X2FI,HAPFIT,DELFIT,RHOF,DFT;
95 double FIFIT,SINF,COSF,SA,SB,SG,SAA,APU,SXYR,ROD1,ROD2,SINF2,COSF2,SIN;
96
97 // check that we have enough points for a reasonable fit
98 if(NP<3) return false;
99 //initialize some variables
100 S1=0.;
101 S2=0.;
102 S3=0.;
103 S4=0.;
104 S5=0.;
105 S6=0.;
106 S7=0.;
107 S8=0.;
108 S9=0.;
109 // IMOD=IABS(MODE)
110 // IF (IMOD.LT.2) THEN
111 int M3=NP/3;
112 // _xx0, _yy0 is a local origin
113 _xx0=XX[M3];
114 _yy0=YY[M3];
115 // DIRTX, DIRTY for direction test
116 double DIRTX=_xx0-XX[0];
117 double DIRTY=_yy0-YY[0];
118 // ELSE //mod=3, i.e prop errors only
119 // DIRTX=COS(_phi)
120 // DIRTY=SIN(_phi)
121 // ENDIF
122 // IF (IMOD.EQ.3) GO TO 50
123 // calculate sums for fit
124
125 for(int IP = 0; IP<NP; ++IP)
126 {
127 XC=XX[IP]-_xx0;
128 YC=YY[IP]-_yy0;
129 WT=WW[IP];
130 WX=WT*XC;
131 WY=WT*YC;
132 RR=XC*XC+YC*YC;
133 WR=WT*RR;
134 S1+=WT;
135 S2+=WX;
136 S3+=WY;
137 S4+=WX*XC;
138 S5+=WX*YC;
139 S6+=WY*YC;
140 S7+=WX*RR;
141 S8+=WY*RR;
142 S9+=WR*RR;
143 }
144 if(S1<0.) return false;
145 //
146 //--- Solve the fitted parameters
147 //
148 S1I=1./S1;
149 SR=S4+S6;
150 HSR=0.5*SR;
151 XMEAN=S1I*S2;
152 YMEAN=S1I*S3;
153 RRMEAN=S1I*SR;
154 COV1=S1I*(S4-S2*XMEAN);
155 COV2=S1I*(S5-S2*YMEAN);
156 COV3=S1I*(S6-S3*YMEAN);
157 COV4=S1I*(S7-S2*RRMEAN);
158 COV5=S1I*(S8-S3*RRMEAN);
159 COV6=S1I*(S9-SR*RRMEAN);
160 if(COV6<0.) return false;
161 Y2FI=2.*(COV2*COV6-COV4*COV5);
162 X2FI=COV6*(COV1-COV3)-COV4*COV4+COV5*COV5;
163 FIFIT=0.5*Math.atan2(Y2FI,X2FI);
164 COSF=Math.cos(FIFIT);
165 SINF=Math.sin(FIFIT);
166 HAPFIT=(SINF*COV4-COSF*COV5)/COV6;
167 DELFIT=-HAPFIT*RRMEAN+SINF*XMEAN-COSF*YMEAN;
168 APU=Math.sqrt(1.-4.*HAPFIT*DELFIT);
169 RHOF=2.*HAPFIT/APU;
170 DFT=2.*DELFIT/(1.+APU);
171 ROD1=1./APU;
172 ROD2=ROD1*ROD1;
173 SINF2=SINF*SINF;
174 COSF2=COSF*COSF;
175 double SINFF=2.*SINF*COSF;
176 SA=SINF*S2-COSF*S3;
177 SAA=SINF2*S4-SINFF*S5+COSF2*S6;
178 SXYR=SINF*S7-COSF*S8;
179 //
180 _rho=RHOF;
181 _phi=FIFIT;
182 _dca=DFT;
183 _chicir=ROD2*(-DELFIT*SA-HAPFIT*SXYR+SAA);
184 _xpca=_xx0+_dca*SINF;
185 _ypca=_yy0-_dca*COSF;
186 //
187 //--- Error estimation ro,fi,d
188 //
189 //IF (MODE.GT.0) THEN
190 SB=COSF*S2+SINF*S3;
191 SG=(SINF2-COSF2)*S5+SINF*COSF*(S4-S6);
192 double W1=.25*S9-DFT*(SXYR-DFT*(SAA+HSR-DFT*(SA-.25*DFT*S1)));
193 double W2=-ROD1*(0.5*(COSF*S7+SINF*S8)-DFT*(SG-0.5*DFT*SB));
194 double W3=ROD2*(COSF2*S4+SINFF*S5+SINF2*S6);
195 double W4=RHOF*(-0.5*SXYR+DFT*SAA)+ROD1*HSR-0.5*DFT*((2.*ROD1+RHOF*DFT)*SA-DFT*ROD1*S1);
196 double W5=ROD1*RHOF*SG-ROD2*SB;
197 double W6=RHOF*(RHOF*SAA-2.*ROD1*SA)+ROD2*S1;
198 double SD1=W3*W6-W5*W5;
199 double SD2=-W2*W6+W4*W5;
200 double SD3=W2*W5-W3*W4;
201 double DETINV=1./(W1*SD1+W2*SD2+W4*SD3);
202 _covrfd[0]=DETINV*SD1;
203 _covrfd[1]=DETINV*SD2;
204 _covrfd[2]=DETINV*(W1*W6-W4*W4);
205 _covrfd[3]=DETINV*SD3;
206 _covrfd[4]=DETINV*(W2*W4-W1*W5);
207 _covrfd[5]=DETINV*(W1*W3-W2*W2);
208 double XDERO=0.5*(RHOF*SXYR-2.*ROD1*SAA+(1.+ROD1)*DFT*SA);
209 double EDERO=DFT*XDERO;
210 double EDEDI=RHOF*XDERO;
211 double DROF=_covrfd[0]*EDERO+_covrfd[3]*EDEDI;
212 double DFIF=_covrfd[1]*EDERO+_covrfd[4]*EDEDI;
213 double DDIF=_covrfd[3]*EDERO+_covrfd[5]*EDEDI;
214 _rho+=DROF;
215 _dca+=DDIF;
216 _phi+=DFIF;
217 _chicir=(1.+_rho*_dca)*(1.+_rho*_dca)*_chicir/ROD2;
218 // ENDIF
219 // 50 CONTINUE propagation of parameters and errors
220 // IF (IMOD.EQ.3) THEN
221 // SINF=SIN(_phi)
222 // COSF=COS(_phi)
223 // ENDIF
224 propagate(_xref, _yref, SINF, COSF, DIRTX, DIRTY);
225 return true;
226
227 }
228
229 /**
230 * Get the results of the fit.
231 * @return the results of the fit in a CircleFit object
232 */
233 public CircleFit getfit()
234 {
235 return new CircleFit(_xref, _yref, _rho, _phi, _dca, _chicir, _covrfd);
236 }
237
238 void propagate(double x, double y, double SINF, double COSF, double DIRTX, double DIRTY)
239 {
240 //
241 //--- Propagate parameters to XREF,YREF
242 //
243 double[] XJACOB = new double[9];
244 double ROD1;
245 // first set _xref, _yref
246 setreferenceposition(x, y);
247 double XMOVE=_xpca-_xref;
248 double YMOVE=_ypca-_yref;
249 ROD1=1.+_rho*_dca;
250 double DPERP=XMOVE*SINF-YMOVE*COSF;
251 double DPARA=XMOVE*COSF+YMOVE*SINF;
252 double ZEE=DPERP*DPERP+DPARA*DPARA;
253 double AA=2.*DPERP+_rho*ZEE;
254 double UU=Math.sqrt(1.+_rho*AA);
255 double SQ1AI=1./(1.+UU);
256 double BB= _rho*XMOVE+SINF;
257 double CC=-_rho*YMOVE+COSF;
258 _phi=Math.atan2(BB,CC);
259 _dca=AA*SQ1AI;
260 //
261 //--- Propagate error matrix to XREF,YREF
262 //
263 //IF (MODE.GT.0) THEN
264 double VV=1.+_rho*DPERP;
265 double XEE=1./(CC*CC+BB*BB);
266 double XLA=0.5*AA*SQ1AI*SQ1AI/UU;
267 double XMU=SQ1AI/UU+_rho*XLA;
268 XJACOB[0]=1.;
269 XJACOB[1]=0.;
270 XJACOB[2]=0.;
271 XJACOB[3]=XEE*DPARA;
272 XJACOB[4]=XEE*ROD1*VV;
273 XJACOB[5]=-XJACOB[3]*_rho*_rho;
274 XJACOB[6]=XMU*ZEE-XLA*AA;
275 XJACOB[7]=2.*XMU*ROD1*DPARA;
276 XJACOB[8]=2.*XMU*VV;
277 // overwrite _covrfd in place
278 abatranspose(XJACOB,_covrfd);
279 SINF=Math.sin(_phi);
280 COSF=Math.cos(_phi);
281 //ENDIF
282 //
283 //--- check direction
284 //
285 double DIRTES=COSF*DIRTX+SINF*DIRTY;
286 if(DIRTES<0.)
287 {
288 _phi=_phi+Math.PI;
289 COSF=-COSF;
290 SINF=-SINF;
291 _dca= -_dca;
292 _rho= -_rho;
293 _covrfd[1]=-_covrfd[1]; //V rho-phi
294 _covrfd[4]=-_covrfd[4]; //V phi-d
295 }
296 if(_phi>=2.*Math.PI) _phi-=2.*Math.PI;
297 if(_phi<0. ) _phi+=2.*Math.PI;
298 _xpca=_xref+_dca*SINF;
299 _ypca=_yref-_dca*COSF;
300 }
301
302 /**
303 * Propagate the fit parameters to a new reference point
304 * @param x x position of the new reference point
305 * @param y y position of the new reference point
306 */
307 public CircleFit propagatefit(double x, double y)
308 {
309 double DIRTX=Math.cos(_phi);
310 double DIRTY=Math.sin(_phi) ;
311 double SINF=Math.sin(_phi);
312 double COSF=Math.cos(_phi);
313 propagate(x, y, SINF, COSF, DIRTX, DIRTY);
314 return new CircleFit(_xref, _yref, _rho, _phi, _dca, _chicir, _covrfd);
315 }
316
317 private void abatranspose(double[] A, double[] B)
318 {
319 /* SUBROUTINE TRASA3(A,B)
320 * *
321 * MATRIX OPERATION ABA'-->B FOR 3X3 MATRICES *
322 * B is a packed symmetric matrix *
323 * A is 3x3 matrix packed row-wise (A' means transpose) *
324 *---------------------------------------------------------------*
325 */
326 double[] C = new double[6];
327 double E1, E2, E3, E4, E5, E6, E7, E8, E9;
328 //for (int i=0; i < 6; ++i) C[i] = B[i];
329 System.arraycopy(B,0,C,0,6);
330 E1=A[0]*C[0]+A[1]*C[1]+A[2]*C[3];
331 E2=A[0]*C[1]+A[1]*C[2]+A[2]*C[4];
332 E3=A[0]*C[3]+A[1]*C[4]+A[2]*C[5];
333 E4=A[3]*C[0]+A[4]*C[1]+A[5]*C[3];
334 E5=A[3]*C[1]+A[4]*C[2]+A[5]*C[4];
335 E6=A[3]*C[3]+A[4]*C[4]+A[5]*C[5];
336 E7=A[6]*C[0]+A[7]*C[1]+A[8]*C[3];
337 E8=A[6]*C[1]+A[7]*C[2]+A[8]*C[4];
338 E9=A[6]*C[3]+A[7]*C[4]+A[8]*C[5];
339 B[0]=A[0]*E1+A[1]*E2+A[2]*E3;
340 B[1]=A[3]*E1+A[4]*E2+A[5]*E3;
341 B[2]=A[3]*E4+A[4]*E5+A[5]*E6;
342 B[3]=A[6]*E1+A[7]*E2+A[8]*E3;
343 B[4]=A[6]*E4+A[7]*E5+A[8]*E6;
344 B[5]=A[6]*E7+A[7]*E8+A[8]*E9;
345 }
346 /*
347 public static void main(String[] args)
348 {
349 CircleFitter fitter = new CircleFitter();
350 double[] xx = new double[40];
351 double[] yy = new double[40];
352 double[] ww = new double[40];
353
354 double[] pulls = new double[3];
355 double[][] pulsum = new double[2][3];
356 double[] pullerr = new double[3];
357 int event = 0;
358 try
359 {
360 FileReader file = new FileReader("fort.4");
361 BufferedReader buff = new BufferedReader(file);
362 boolean eof = false;
363 while(!eof)
364 {
365 ++event;
366 for (int i = 0; i<40; ++i)
367 {
368 String line = buff.readLine();
369 if (line == null)
370 {
371 eof = true;
372 break;
373 }
374 else
375 {
376 //System.out.println(line);
377 StringTokenizer st = new StringTokenizer(line);
378 xx[i] = Double.parseDouble(st.nextToken());
379 yy[i] = Double.parseDouble(st.nextToken());
380 ww[i] = Double.parseDouble(st.nextToken());
381 }
382 }
383 if(!eof)
384 {
385 System.out.println(" Event "+ event);
386 boolean OK=fitter.fit(xx,yy,ww,40);
387 if(OK)
388 {
389 // System.out.println(fitter.getfit());
390 CircleFit cf = fitter.getfit();
391 double[] covmat = cf.cov();
392 double ROERR=Math.sqrt(covmat[0]);
393 double FIERR=Math.sqrt(covmat[2]);
394 double DCERR=Math.sqrt(covmat[5]);
395
396 pulls[0]=(0.01-cf.curvature())/ROERR;
397 pulls[1]=(1.23-cf.phi())/FIERR;
398 pulls[2]=(0.1-cf.dca())/DCERR;
399
400 for(int i =0; i<3;++i)
401 {
402 // System.out.println("pulls["+i+"]= "+pulls[i]);
403 pulsum[0][i]+=pulls[i];
404 pulsum[1][i]+=pulls[i]*pulls[i];
405 }
406 //test propagate method...
407 double XTEST = 0.1*Math.sin(1.23);
408 double YTEST = -0.1*Math.cos(1.23);
409 XTEST = 0.;
410 YTEST = 0.;
411 System.out.println("XTEST= "+XTEST+", YTEST= "+YTEST);
412 fitter.propagatefit(XTEST, YTEST);
413 CircleFit cf2 = fitter.getfit();
414 System.out.println(cf);
415 System.out.println(cf2);
416 }
417 eof = true;
418 }
419 }
420
421 }
422 catch (IOException e)
423 {
424 System.out.println("Error -- " + e.toString());
425 }
426 double FACTO=1./Math.sqrt((double)event);
427 //
428 //--- Calculate means and std's of the pull values
429 //
430
431 for (int i=0; i<3; ++i)
432 {
433 pulsum[0][i]=pulsum[0][i]/(double)event;
434 pulsum[1][i]=Math.sqrt((pulsum[1][i])/(double)event-(pulsum[0][i]*pulsum[0][i]));
435 pullerr[i]=FACTO*pulsum[1][i];
436 }
437 for (int i = 0; i<3; ++i)
438 {
439 double TEST = Math.abs(pulsum[0][i]/pullerr[i]);
440 System.out.println(pulsum[0][i]+" "+pullerr[i]+" "+pulsum[1][i]+" "+TEST);
441 }
442 }
443 */
444 }
445 /**************** END OF CIRCLE FITTING CODE *********************/