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 }