View Javadoc

1   package org.lcsim.fit.circle;
2   // Circle Fitting Classes based on Karimaki's fast circle fit.
3   /*
4   $!       **************************************************
5   $!       *          CIRCLE FIT SAMPLE PROGRAM             *
6   $!       **************************************************
7   $ CREATE CIRCLEFIT.FOR
8    ******************************************************************
9         PROGRAM CIRCLEFIT
10   *     ******* *********            4.6.1991   V. Karimaki        *
11   *                                                                *
12   *     Main program to test the circle fitting routine CIRCLF     *
13   *                                                                *
14   *     Circle data points are Monte Carlo generated and then      *
15   *     used to test CIRCLF NTRGEN times. The pull values are      *
16   *     calculated for the three fitted parameters.                *
17   *                                                                *
18   *     The test is passed, if the statistical distributions of    *
19   *     the pull values follow the normal distribution. The means  *
20   *     and standard deviations of the three pull values are       *
21   *     printed in the test run output.                            *
22   *                                                                *
23   *     Constants (units are supposed to be in metres):            *
24   *     AVEDEV = average residual                                  *
25   *     STEP   = spacing between points along circle               *
26   *     RHOMAX = maximal (absolute) value of curvature             *
27   *     DISRMX = maximal ratio |DCA|/track length                  *
28   *----------------------------------------------------------------*
29   */
30  import java.util.Random;
31  
32  class CircleFitTest
33  {
34      double[] _xx; //array of simulated x points
35      double[] _yy; //array of simulated y points
36      double[] _wxy; //array of weights
37      double[] _xyr; //array of radial positions
38      Random _r;
39      int _ncmax;
40      CircleFitter _fitter;
41      
42      double[] _pulls;
43      double[][] _pulsum;
44      double[] _pullerr;
45      
46      // Constructor
47      CircleFitTest()
48      {
49          _r = new Random();
50          _fitter = new CircleFitter();
51          _ncmax = 500;
52          _xx = new double[_ncmax];
53          _yy = new double[_ncmax];
54          _wxy = new double[_ncmax];
55          _xyr = new double[_ncmax];
56          _pulls = new double[3];
57          _pulsum = new double[2][3];
58          _pullerr = new double[3];
59          double tperpo[] = new double[2];
60          double nptest[] = new double[4];
61      }
62      //      PARAMETER (NRAND=1000)
63      //      COMMON/RANTAB/ LUNI,LGAU,UNITAB(NRAND),GAUTAB(NRAND)
64      //      PARAMETER (NCMAX=500)
65      //      COMMON /CIRPOI/ XX(NCMAX),YY(NCMAX),WXY(NCMAX),XYR(NCMAX)
66      //      COMMON /CIRCFI/ RHO,PHI,DCA,CHICIR,XPCA,YPCA,COVRFD(6)
67      //     +,          XX0,YY0,S1,S2,S3,S4,S5,S6,S7,S8,S9
68      //      REAL*8          S1,S2,S3,S4,S5,S6,S7,S8,S9
69      //      DIMENSION PULLS(3),PULSUM(2,3),PULERR(3)
70      //      DIMENSION TPERPO(2),NPTEST(4)
71      //      DATA NPTEST /4,20,100,500/
72      //      DATA AVEDEV /.00030/,    STEP/0.010/
73      //      DATA RHOMAX,DISRMX/ 1.00, 0.40/
74      //      PI2=8.*ATAN(1.)
75      
76      
77      void test()
78      {
79          //
80          //--- Number of generations
81          //
82          int NTRGEN=2000;
83          int NPRINT=10;
84          int NMODUL=NTRGEN/NPRINT;
85          
86          double step = 0.01;
87          double AVEDEV = 0.00030;
88          double RHOMAX = 1.0;
89          double DISRMX=0.40;
90          
91          System.out.println("TEST RUN OUTPUT");
92          System.out.println("Circle fit routine is tested with "+NTRGEN+
93                  " randomly generated sets of circle data.");
94          //
95          //--- Loop to generate circle points and test the routine CIRCLF
96          //
97          //DO 100 ITR=1,NTRGEN
98          for(int ITR=0; ITR<NTRGEN; ++ITR)
99          {
100             //--- Generate random curvature, direction and dca
101             
102             double RND1=_r.nextDouble();
103             double RND2=_r.nextDouble();
104             double RND3=_r.nextDouble();
105             double RND4=_r.nextDouble();
106             
107             double RHOGEN=2.*RHOMAX*(RND1-0.5);
108             double PHIGEN=2.*Math.PI*RND2;
109             double AROGEN=Math.abs(RHOGEN);
110             //
111             //--- Number of points on circle  4<=NPO<=300
112             //
113             int NPO=4+(int)(296*RND4);
114             double XLENG=(NPO-1)*step;
115             double DISGEN=DISRMX*2.*XLENG*(RND3-0.5);
116             if (AROGEN>0.)
117             {
118                 double RADIUS=1./AROGEN;
119                 double DISLIM=0.75*RADIUS;
120                 if (Math.abs(DISGEN)>DISLIM)
121                 {
122                     DISGEN = DISLIM;
123                     if(DISGEN<0) DISGEN = -DISLIM;
124                 }
125                 int NPFULL=(int)(6.2832*RADIUS/step-2);
126                 NPO=Math.min(NPO,NPFULL);
127             }
128             double XREFE=RND1-0.5;
129             double YREFE=RND2-0.5;
130             //CNG
131             // if I fix these parameters, and calculate pulls
132             // with respect to these fixed parameters, everything
133             // is fine. When I don't everything goes to hell.
134             // Error must be in coding of modifications, or in propagate
135             // method.
136             // May, 18, 2000
137             // I have now fixed the propagate method.
138             // Everything works well if I fix these starting parameters
139             // and propagate to a random XTEST, YTEST
140                         /*
141                         NPO = 40;
142                         RHOGEN = 0.01;
143                         PHIGEN = 1.23;
144                         DISGEN = 0.1;
145                         XREFE = 0.;
146                         YREFE = 0.;
147                          */
148             //CNG
149             double SINGEN=Math.sin(PHIGEN);
150             double COSGEN=Math.cos(PHIGEN);
151             double XSTART= XREFE+DISGEN*SINGEN;
152             double YSTART= YREFE-DISGEN*COSGEN;
153             //
154             //--- Generate a circle with random parameters
155             //
156             simCirclePoints(RHOGEN,PHIGEN,XSTART,YSTART,NPO,AVEDEV,step);
157             //Check the generation here...
158             //System.out.println("phigen= "+PHIGEN+", rhogen= "+RHOGEN+", dca= "+DISGEN);
159                         /*for(int i = 0; i<NPO; ++i)
160                          {
161                                 System.out.println("_xx["+i+"]= "+_xx[i]+", _yy["+i+"]= "+_yy[i]+", _wxy["+i+"]= "+_wxy[i]);
162                          }
163                          */
164             
165             //
166             //--- Perform circle fitting with error estimation
167             //
168             //         CALL CIRCLF(XREFE,YREFE,XX,YY,WXY,NPO,1,IER)
169             _fitter.setreferenceposition( XREFE, YREFE);
170             //System.out.println("xref= "+XREFE+", yref= "+YREFE);
171             boolean OK = _fitter.fit( _xx, _yy, _wxy,NPO);
172             //if(OK) System.out.println("Fit OK");
173             CircleFit cf = _fitter.getfit();
174             //System.out.println(cf);
175             
176             //
177             //--- Define also another reference point to test propagation
178             double XTEST=XSTART+DISGEN*(RND3-0.5);
179             double YTEST=YSTART+DISGEN*(RND4-0.5);
180             //
181             //--- Calculate the true parameters at the propagation test point
182             //
183             double XMOVE=XREFE-XTEST;
184             double YMOVE=YREFE-YTEST;
185             double ROD1=1.+RHOGEN*DISGEN;
186             double DPERP=XMOVE*SINGEN-YMOVE*COSGEN + DISGEN;
187             double DPARA=XMOVE*COSGEN+YMOVE*SINGEN;
188             double AA=2.*DPERP+RHOGEN*(DPERP*DPERP+DPARA*DPARA);
189             double UU=Math.sqrt(1.+RHOGEN*AA);
190             double SQ1AI=1./(1.+UU);
191             double BB= RHOGEN*XMOVE+ROD1*SINGEN;
192             double CC=-RHOGEN*YMOVE+ROD1*COSGEN;
193             double RHOTES=RHOGEN;
194             double PHITES=Math.atan2(BB,CC);
195             double DISTES=AA*SQ1AI;
196             //
197             //--- Propagate to a test point
198             //
199             _fitter.propagatefit(XTEST, YTEST);
200             cf = _fitter.getfit();
201             
202             double FIC=cf.phi();
203             if(PHITES-cf.phi()>Math.PI) FIC+=2.*Math.PI;
204             if(cf.phi()-PHITES>Math.PI) FIC-=2.*Math.PI;
205             double[] covmat = cf.cov();
206             double ROERR=Math.sqrt(covmat[0]);
207             double FIERR=Math.sqrt(covmat[2]);
208             double DCERR=Math.sqrt(covmat[5]);
209             //
210             //--- Print a few fit results
211             //
212                         /*
213                         IF (MOD(ITR,NMODUL).EQ.1) THEN
214                         WRITE(IUNIT,1005) RHO,ROERR,FIC,FIERR,DCA,DCERR,CHICIR,NPO
215                         WRITE(IUNIT,1006) RHOTES,PHITES,DISTES
216                         1005       FORMAT('    Fitted',3(2X,F7.4,'+-',F6.4),4X,F5.1,I6)
217                         1006       FORMAT('    True  ',3(2X,F7.4,8X))
218                         ENDIF
219                          */
220             //
221             //--- Calculate the pull values
222             //
223             // use when I have implemented propagate...
224             _pulls[0]=(RHOTES-cf.curvature())/ROERR;
225             _pulls[1]=(PHITES-FIC)/FIERR;
226             _pulls[2]=(DISTES-cf.dca())/DCERR;
227             
228             for(int i =0; i<3;++i)
229             {
230                 //	System.out.println("pulls["+i+"]= "+_pulls[i]);
231                 _pulsum[0][i]+=_pulls[i];
232                 _pulsum[1][i]+=_pulls[i]*_pulls[i];
233             }
234             
235         }
236         
237         //
238         //--- Test run output
239         //
240         double FACTO=1./Math.sqrt((double)(NTRGEN));
241         //
242         //--- Calculate means and std's of the pull values
243         //
244         System.out.println("Mean +/- error and standard deviation of pulls");
245         for (int i=0; i<3; ++i)
246         {
247             _pulsum[0][i]=_pulsum[0][i]/(double)NTRGEN;
248             _pulsum[1][i]=Math.sqrt(_pulsum[1][i]/((double)NTRGEN)-_pulsum[0][i]*_pulsum[0][i]);
249             _pullerr[i]=FACTO*_pulsum[1][i];
250             System.out.println("Mean: "+_pulsum[0][i]+" +/- "+_pullerr[i]+" sigma= "+_pulsum[1][i]);
251         }
252                 /*
253                 WRITE(IUNIT,1010)
254                 1010 FORMAT(///,17X,'Means and standard deviations of the pull values
255                 +,//,20X,'Circle parameter','   Pull mean       Pull std',/)
256                 WRITE(IUNIT,1011) pulsum(1,1),pullerr(1),pulsum(2,1)
257                 WRITE(IUNIT,1012) pulsum(1,2),pullerr(2),pulsum(2,2)
258                 WRITE(IUNIT,1013) pulsum(1,3),pullerr(3),pulsum(2,3)
259                 1011 FORMAT(22X,'Curvature',5X,F7.3,'+-',F5.3,5X,F6.3)
260                 1012 FORMAT(22X,'Angle    ',5X,F7.3,'+-',F5.3,5X,F6.3)
261                 1013 FORMAT(22X,'Distance ',5X,F7.3,'+-',F5.3,5X,F6.3)
262                 //
263                 //--- Means and std's should be those of the normal distribution
264                 //
265                 IOK=1
266                 DO 140 IPL=1,3
267                 TEST=ABS(pulsum(1,IPL)/pullerr(IPL))
268                 IF (TEST.GT.3.) IOK=0
269                 TEST=ABS(pulsum(2,IPL)-1.)
270                 IF (TEST.GT..2) IOK=0
271                 140 CONTINUE
272                 IF (IOK.EQ.1) WRITE(IUNIT,1020)
273                 IF (IOK.EQ.0) WRITE(IUNIT,1021)
274                 1020 FORMAT(/,16X,' Pulls test is passed OK.')
275                 1021 FORMAT(/,16X,' Something wrong: pulls are not normally'
276                 +,' distributed.')
277                  */
278                 /*
279                  for (int i = 0; i<3; ++i)
280                  {
281                    double TEST = Math.abs(_pulsum[0][i]/_pullerr[i]);
282                 System.out.println("Pull Test["+i+"]= "+TEST);
283                  }
284                  */
285         
286         //
287         //--- Timing tests
288         //
289         //
290         //--- Generate a circle with random parameters with 500 points
291         //
292         simCirclePoints(.01,1.23,0.009,0.0033,500,0.0003,0.01);
293         int[] nptest = {4, 20, 100, 500};
294         long[] times = new long[nptest.length];
295         _fitter.setreferenceposition( 0., 0.);
296         //Fit this circle a number of times and save times as
297         //a function of the number of points fit.
298         for(int i=0; i<nptest.length; ++i)
299         {
300             long t1 = System.currentTimeMillis();
301             int ndo = 5000;
302             for(int j=0; j<ndo; ++j)
303             {
304                 boolean OK = _fitter.fit( _xx, _yy, _wxy,nptest[i]);
305             }
306             times[i] = System.currentTimeMillis()-t1;
307             System.out.println("It took "+(double)times[i]/(double)(ndo*nptest[i])+" milliseconds for circles with "+nptest[i]+" hits");
308         }
309         
310                 /*
311                 CALL CIRGEN(RHOGEN,PHIGEN,XSTART,YSTART,500,AVEDEV,STEP)
312                 WRITE(IUNIT,2000)
313                 2000 FORMAT(////,16X,' Timing tests WITHOUT/WITH error estimation:'
314                 +,//,26X,'#Points',5X,'Time per point (us)',/)
315                 DO 400 JP=1,4
316                 NPO=NPTEST(JP)
317                 NDO= 25000/(30+NPO)
318                  *
319                  *--- II=1 without error estimation II=2 with
320                  *
321                 DO 300 II=1,2
322                 IF (II.EQ.1) IES=-1
323                 IF (II.EQ.2) IES=+1
324                 CALL TIMVX(TSTART)
325                 TSTART=1000*TSTART
326                  *
327                  *---- call circle fitting NDO times for timing
328                  *
329                 DO 200 I=1,NDO
330                 CALL CIRCLF(XREFE,YREFE,XX,YY,WXY,NPO,IES,IER)
331                 200       CONTINUE
332                 CALL TIMVX(TEND)
333                 TEND=1000*TEND
334                 TPERPO(II)=1000*(TEND-TSTART)/NDO/NPO
335                 300    CONTINUE
336                 WRITE(IUNIT,2001) NPO,TPERPO
337                 2001    FORMAT(26X,I6,7X,F6.1,' /',F6.1)
338                 400 CONTINUE
339                 STOP
340                 END
341                  
342                  */
343     }
344     void simCirclePoints(double rho, double phi, double x, double y,
345             int npoints, double avedev, double step)
346     {
347                 /*
348                  *****************************************************************
349                 SUBROUTINE CIRGEN(RHO,PHI,X,Y,NPO,AVEDEV,STEP)
350                  *                                                               *
351                  *     SUBROUTINE TO GENERATE MEASURED POINTS ALONG CIRCLE       *
352                  *     POINTS ARE GAUSSIAN FLUCTUATED ABOUT THE TRUE CIRCLE.     *
353                  *                                                               *
354                  *     THE ROUTINE IS PART OF THE CIRCLE FIT TEST PROGRAM        *
355                  *                                                               *
356                  *     INPUT PARAMETERS:                                         *
357                  *     RHO    = SIGNED CURVATURE (+VE BENDING CLOCKWISE)         *
358                  *     PHI    = DIRECTION ANGLE IN XY-PROJECTION                 *
359                  *     X,Y    = STARTING POINT COORDINATES                       *
360                  *     NPO    = NUMBER OF POINTS TO GENERATE                     *
361                  *     AVEDEV  = MEAN DEVIATION NORMAL TO TRACK                  *
362                  *     STEP   = STEP LENGTH ALONG TRACK                          *
363                  *                                                               *
364                  *     OUTPUT PARAMETERS:                                        *
365                  *     OUTPUT COORDINATES ARE IN ARRAYS XX,YY                    *
366                  * --------------------------------------------------------------*
367                  */
368                 /*
369                 PARAMETER (NRAND=1000)
370                 COMMON/RANTAB/ LUNI,LGAU,UNITAB(NRAND),GAUTAB(NRAND)
371                 PARAMETER (NCMAX=500)
372                 COMMON /CIRPOI/ XX(NCMAX),YY(NCMAX),WXY(NCMAX),XYR(NCMAX)
373                 REAL*8 PHIS,HDPHI,CORD,XS,YS
374                 C-
375                  */
376         double HDPHI   = -rho*step/2.0;
377         double CORD = step;
378         //     if(Math.abs(HDPHI)<1.0e-5f) double CORD = step;
379         if(Math.abs(HDPHI)>=1.0e-5f) CORD = 2.*Math.sin(-HDPHI)/rho;
380         double XS = x;  // x start
381         double YS = y;  // y start
382         double PHIS = phi; // phi start
383         
384         // GENERATE POINTS ALONG CIRCLE
385         for(int NP=0; NP<npoints; ++NP)
386         {
387             double AVERES = (0.5+_r.nextDouble())*avedev;
388             double RESIXY = _r.nextGaussian()*AVERES;
389             
390             // FLUCTUATE NORMAL TO TRAJECTORY AND STORE POINT
391             _xx[NP] = XS + Math.sin(PHIS)*RESIXY;
392             _yy[NP] = YS - Math.cos(PHIS)*RESIXY;
393             _wxy[NP]= 1./(AVERES*AVERES);
394             _xyr[NP]= RESIXY;
395             
396             // STEP TO THE NEXT POINT ON CIRCLE
397             XS     +=  CORD*Math.cos(PHIS+HDPHI);
398             YS     +=  CORD*Math.sin(PHIS+HDPHI);
399             PHIS   +=  2.*HDPHI;
400         }
401         
402     }
403     public static void main(String[] args)
404     {
405         CircleFitTest t = new CircleFitTest();
406         t.test();
407     }
408 }