program fit_ellipse implicit none ! tests the ellipse specific fitting ! for generating and storing the noisy data integer n,nmax parameter (nmax=100) integer i double precision x(nmax+3),y(nmax+3),sig(nmax+3), & x0,y0,tilt,co,si,angmin,angmax, & ampl,rmaj,rmin,rvec(2) ! for the ellipse fitter double precision xcenter,ycenter,ttilt,semi_major,semi_minor, & eccen,xfoci1,yfoci1,xfoci2,yfoci2,coef(6) ! for writing out the fitted conic character*40 string integer jend double precision xxx(nmax+3),yyy(nmax+3) ! popular formats 01 format(1x,a,1p10e12.4) 02 format(a,i3.3,a) 03 format(1x,1pe11.3,' * x**2 +'/, & 1x,1pe11.3,' * y**2 +',/, & 1x,1pe11.3,' * x*y +',/, & 1x,1pe11.3,' * x +',/, & 1x,1pe11.3,' * y +',/, & 1x,1pe11.3,' = 0') 04 format(1x,1p2e12.4) ! seed the random number generator call random_seed(put=(/ 123456789, 987654321 /)) ! generate a noisy data set x0 = 0.75d0 y0 = 0.25d0 rmaj = 1.25d0 rmin = 0.75d0 tilt = 30.0d0 angmin = 0.0d0 angmax = 360.0d0 ampl = 0.05d0 n = 40 call generate_noisy_ellipse(x0,y0,rmaj,rmin,tilt,angmin,angmax,ampl,n,x,y,sig) ! fit an ellipse call fit_conic_from_specific(x, y, n, 'E', & ttilt,xcenter,ycenter,semi_major,semi_minor, & eccen,xfoci1,yfoci1,xfoci2,yfoci2,coef) ! report the results write(6,*) ' ' write(6,*) 'geometric results of fitting the data to an ellipse:' write(6,01) 'tilt :',ttilt write(6,01) 'center at :',xcenter,ycenter write(6,01) 'smajor axis:',semi_major write(6,01) 'sminor axis:',semi_minor write(6,01) 'eccentric :',eccen write(6,01) 'foci1 at :',xfoci1,yfoci1 write(6,01) 'foci2 at :',xfoci2,yfoci2 write(6,*) ' ' write(6,01) 'equation of best fit conic:' write(6,03) coef ! store the fitted curve ! put the center and two foci as the first three points xxx(1) = xcenter yyy(1) = ycenter xxx(2) = xfoci1 yyy(2) = yfoci1 xxx(3) = xfoci2 yyy(3) = yfoci2 jend = min(361,nmax) angmin = 0.0d0 angmax = 360.0d0 ampl = 0.0d0 call generate_noisy_ellipse(xcenter,ycenter,semi_major,semi_minor,ttilt, & angmin,angmax,ampl,jend,xxx(4),yyy(4),sig(4)) ! write out the random points and the best fit for plotting write(string,02) 'noisy_data_',int(tilt),'.dat' open (unit=3,file=string,status='unknown') write(3,04) (x(i), y(i), i=1,n) close(unit=3) write(string,02) 'curve_',int(tilt),'.dat' open (unit=3,file=string,status='unknown') write(3,04) (xxx(i), yyy(i), i=1,jend+3) close(unit=3) end subroutine fit_conic_from_specific(x, y, n, conic_type, & tilt,x0,y0,rmaj,rmin, & eccen,xfoci1,yfoci1,xfoci2,yfoci2,coef) implicit none ! this routine fits data to an ellipse specific conic ! a*x**2 + b*y**2 + c*x*y + d*x + e*y + f = 0 ! by minimizing the algebraic distance ! references: ! "numerically stable direct least squares fitting of ellipses" ! halir, r., flusser, j., 1998. ! proceedings of the 6th international conference ! in central europe on computer graphics and visualization. ! wscg ‘98. cz, plzeo, 1998, pp. 125–132. ! ! "direct and specific least-square fitting of hyperbolae and ellipses" ! o'leary, p and zsombar-urray, p. ! journal of electronic imaging, 13, 3, 2004, pages 492-503 ! ! "direct type-specific conic fitting and eigenvalue bias correction" ! harker, m. o'leary, p. and zsombor-murray, p. ! image vision computing, 26, 3, 2008, pages 372-381 ! input: ! x(1:n) = x-coordinates of the data ! y(1:n) = y-coordinates of the data ! n = number of data points ! conic_type = 'E' for ellipse or 'H' for hyperbola ! output: ! x0 = x coordinate of the ellipse center ! y0 = y coordinate of the ellipse center ! xfoci1 = x coordinate of foci1 ! yfoci1 = y coordinate of foci1 ! xfoci2 = x coordinate of foci2 ! yfoci2 = y coordinate of foci2 ! rmaj = length of semi-major axis ! rmin = length of semi-minor axis ! tilt = angle between the major axis and the x-axis ! eccen = eccentricity ! coef(6) = conic equation coefficients ! this routine depends on the lapack routines dgetrf and dgetri ! to invert a 3x3 matrix and the routine dgeev to obtain the eigensystem. ! you'll need to link into the lapack & blas libraries on your system. ! declare the pass character*1 conic_type integer n double precision x(n),y(n),x0,y0,xfoci1,yfoci1,xfoci2,yfoci2, & rmaj,rmin,tilt,eccen,coef(6) ! local variables integer nmax,npar,lwork parameter (nmax=500, npar=3, lwork = npar * 16) integer i,j,ipiv(npar),info,iat(1),jat(1) double precision d1(nmax,npar),d2(nmax,npar),s1(npar,npar),s2(npar,npar),s3(npar,npar), & amat(npar,npar),t(npar,npar),m(npar,npar),cmat(npar,npar), & wr(npar),wi(npar),vl(npar,npar),vr(npar,npar),a1(npar),a2(npar),work(lwork), & cond(npar),bigj,co,si,ar,cr,dr,er,fr,fff,t1,t2,a,b,c,d,e,f double precision ellipse_coef(2*npar),hyperbola_coef(2*npar),possib(npar-1) ! for normalizing the data before fitting double precision xn(nmax),yn(nmax),mx,my,sx,sy ! bullet check if (n .gt. nmax) stop 'n > nmax' if (conic_type .ne. 'E' .and. conic_type .ne. 'H') stop 'bad conic_type' ! normalized the data per harker et al 2008 mx = sum(x)/dfloat(n) my = sum(y)/dfloat(n) sx = sum(x*x + y*y) / (2.0d0 * dfloat(n)) sy = sx xn = (x - mx)/sx yn = (y - my)/sy ! set the quadratic and linear parts of design matrix d1(1:n,1) = xn**2 d1(1:n,2) = xn*yn d1(1:n,3) = yn**2 d2(1:n,1) = xn d2(1:n,2) = yn d2(1:n,3) = 1.0d0 ! form the scatter matrices s1, s2, and s3 s1 = matmul(transpose(d1),d1) s2 = matmul(transpose(d1),d2) s3 = matmul(transpose(d2),d2) ! use lapack routines to invert s3 and form the t matrix amat = s3 call dgetrf(npar, npar, amat, npar, ipiv, info) if (info .ne.0) stop 'info ne 0 from dgetrf in ellipse_from_specific' call dgetri(npar, amat, npar, ipiv, work, lwork, info) if (info .ne.0) stop 'info ne 0 from dgetri in routine ellipse_from_specific' t = matmul(amat, transpose(s2)) ! the constraint matrix c and its inverse cmat = 0.0d0 cmat(1,3) = -2.0d0 cmat(2,2) = 1.0d0 cmat(3,1) = -2.0d0 amat = 0.0d0 amat(1,3) = -0.5d0 amat(2,2) = 1.0d0 amat(3,1) = -0.5d0 ! reduced scatter matrix m = matmul(amat,(s1 - matmul(s2,t))) ! use lapack to get the eigens of the reduced scatter matrix call dgeev('N','V',npar,m,npar,wr,wi,vl,npar,vr,npar,work,lwork,info) ! form the condition numbers b*b - 4ac for each eigenvector and sort 'em cond = vr(2,:)*vr(2,:) - 4.0d0*vr(1,:)*vr(3,:) call indexx(npar,cond,ipiv) ! pick up the elliptical coefficients ! the only negative condition number iat = ipiv(1) a1 = vr(:,ipiv(1)) a2 = matmul(-t,a1) ellipse_coef = (/ a1, a2 /) call denormalize(ellipse_coef,mx,my,sx,sy) ! pick up the hyperbola coefficients ! location of the positive condition number value that ! is closest to the ellipse condition number possib = (/cond(ipiv(2)) , cond(ipiv(3))/) + cond(ipiv(1)) possib(1) = abs(possib(1)) possib(2) = abs(possib(2)) iat = minloc(possib) a1 = vr(:,ipiv(iat(1)+1)) a2 = matmul(-t,a1) hyperbola_coef = (/ a1, a2 /) call denormalize(hyperbola_coef,mx,my,sx,sy) ! figure out the geometrical properties of the conic if (conic_type .eq. 'E') then coef = ellipse_coef call ellipse_geometry_from_coefs(coef,x0,y0,tilt,rmaj,rmin,eccen,xfoci1,yfoci1,xfoci2,yfoci2) else if (conic_type .eq. 'H') then coef = hyperbola_coef call hyperbola_geometry_from_coefs(coef,x0,y0,tilt,rmaj,rmin,eccen,xfoci1,yfoci1,xfoci2,yfoci2) end if return end subroutine denormalize(coef,mx,my,sx,sy) implicit none ! denormalize a set of conic coefficients coeff ! from the data set's computed mean values, mx and my, ! and standard deviations, sx and sy. ! declare the pass double precision coef(6),mx,my,sx,sy ! local variables double precision a(6) ! copy the coefficients a = coef(1:6) ! and reset them coef(1) = a(1)*sy*sy coef(2) = a(2)*sx*sy coef(3) = a(3)*sx*sx coef(4) = -2.0d0*a(1)*sy*sy*mx - a(2)*sx*sy*my + a(4)*sx*sy*sy coef(5) = -a(2)*sx*sy*mx - 2.0d0*a(3)*sx*sx*my + a(5)*sx*sx*sy coef(6) = a(1)*sy*sy*mx*mx + a(2)*sx*sy*mx*my + a(3)*sx*sx*my*my & - a(4)*sx*sy*sy*mx - a(5)*sx*sx*sy*my + a(6)*sx*sx*sy*sy return end subroutine generate_noisy_ellipse(x0,y0,rmaj,rmin,tilt,angmin,angmax,ampl,n,x,y,sig) implicit none ! generates a noisy ellipse ! input: ! x0 = x coordinate of the ellipse center ! y0 = y coordinate of the ellipse center ! tilt = angle between the major axis and the x-axis in degrees ! rmaj = length of semi-major axis ! rmin = length of semi-minor axis ! angmin = minimum angle in degrees to start the noisy circle fro ! angmax = maximim angle in degrees to end the noisy circle from ! ampl = amplitude of the gaussian noie to be added - set to 0.0 for a perfect ellipse ! n = number of data points to generate ! output ! x = x-coordinate of data ! y = y-coordinate of data ! sig = gaussian uncertainty in the data ! declare the pass integer n double precision x0,y0,rmaj,rmin,tilt,angmin,angmax,ampl,x(n),y(n),sig(n) ! local variables integer i double precision co,si,dangle,angle,xx,yy,rvec(2) ! go co = cosd(tilt) si = sind(tilt) dangle = (angmax - angmin)/float(n-1) do i = 1, n angle = angmin + dangle*dfloat(i-1) xx = rmaj * cosd(angle) yy = rmin * sind(angle) call gasdev(rvec) x(i) = co*xx - si*yy + x0 + ampl*rvec(1) y(i) = si*xx + co*yy + y0 + ampl*rvec(2) sig(i) = ampl if (ampl .eq. 0.0) sig(i) = 1.0d0 enddo return end subroutine ellipse_geometry_from_coefs(coef, & x0,y0,tilt,rmaj,rmin,eccen,xfoci1,yfoci1,xfoci2,yfoci2) implicit none ! input: ! coef = the six coeficients to the conic equation ! coef(1)*x**2 + coef(2)*y**2 + coef(3)*x*y + coef(4)*x + coef(5)*y + coef(6) = 0 ! ! output: ! x0 = x coordinate of the ellipse center ! y0 = y coordinate of the ellipse center ! tilt = angle between the major axis and the x-axis ! rmaj = length of semi-major axis ! rmin = length of semi-minor axis ! eccen = eccentricity ! xfoci1 = x coordinate of foci1 ! yfoci1 = y coordinate of foci1 ! xfoci2 = x coordinate of foci2 ! yfoci2 = y coordinate of foci2 ! declare the pass double precision coef(6),x0,y0,tilt,rmaj,rmin,eccen,xfoci1,yfoci1,xfoci2,yfoci2 ! local variables double precision a,b,c,d,e,f,co,si,ar,cr,dr,er,fr,bigj,fff,t1,t2,foci_dist ! popular format statements 10 format(1x,1p10e14.6) ! put the coefficients into their classical form a = coef(1) b = coef(2) c = coef(3) d = coef(4) e = coef(5) f = coef(6) ! tilt - we may be off by 90 degrees but we'll adjust later tilt = 0.5d0 * atand(b/(a - c)) co = cosd(tilt) si = sind(tilt) ! conic coefficients in the rotated frame ar = a*co*co + b*si*co + c*si*si cr = a*si*si - b*si*co + c*co*co dr = d*co + e*si er = -d*si + e*co fr = f ! center bigj = b*b - 4.0d0*a*c x0 = (2.0d0*c*d - b*e) / bigj y0 = (2.0d0*a*e - b*d) / bigj ! semi-major and semi-minor axes fff = (c*d*d + a*e*e - b*d*e + b*b*f - 4.0d0*a*c*f)/bigj t1 = sqrt(abs(fff/ar)) t2 = sqrt(abs(fff/cr)) ! if the tilt angle was correct if (t1 .gt. t2) then rmaj = t1 rmin = t2 ! if the tilt angle is not correct, swap the major and minor axes ! and adjust the tilt else rmaj = t2 rmin = t1 tilt = tilt + 90.0d0 co = cosd(tilt) si = sind(tilt) end if ! distance from center to the foci and the eccentricity foci_dist = sqrt(rmaj**2 - rmin**2) eccen = foci_dist/rmaj ! coordinates of the two foci xfoci1 = x0 + co*foci_dist yfoci1 = y0 + si*foci_dist xfoci2 = x0 - co*foci_dist yfoci2 = y0 - si*foci_dist return end subroutine hyperbola_geometry_from_coefs(coef, & x0,y0,tilt,rmaj,rmin,eccen,xfoci1,yfoci1,xfoci2,yfoci2) implicit none ! input: ! coef = the six coeficients to the conic equation ! coef(1)*x**2 + coef(2)*y**2 + coef(3)*x*y + coef(4)*x + coef(5)*y + coef(6) = 0 ! ! output: ! x0 = x coordinate of the hyperbola center ! y0 = y coordinate of the hyperbola center ! tilt = angle between the major axis and the x-axis ! rmaj = length of semi-major axis ! rmin = length of semi-minor axis ! eccen = eccentricity ! xfoci1 = x coordinate of foci1 ! yfoci1 = y coordinate of foci1 ! xfoci2 = x coordinate of foci2 ! yfoci2 = y coordinate of foci2 ! declare the pass double precision coef(6),x0,y0,tilt,rmaj,rmin,eccen,xfoci1,yfoci1,xfoci2,yfoci2 ! local variables double precision a,b,c,d,e,f,co,si,ar,cr,dr,er,fr,bigj,fff,t1,t2,foci_dist ! put the coefficients into their classical form a = coef(1) b = coef(2) c = coef(3) d = coef(4) e = coef(5) f = coef(6) ! tilt - we may be off by 90 degrees but we'll adjust later tilt = 0.5d0 * atand(b/(a - c)) co = cosd(tilt) si = sind(tilt) ! conic coefficients in the rotated frame ar = a*co*co + b*si*co + c*si*si cr = a*si*si - b*si*co + c*co*co dr = d*co + e*si er = -d*si + e*co fr = f ! center bigj = b*b - 4.0d0*a*c x0 = (2.0d0*c*d - b*e) / bigj y0 = (2.0d0*a*e - b*d) / bigj ! semi-major and semi-minor axes fff = (c*d*d + a*e*e - b*d*e + b*b*f - 4.0d0*a*c*f)/bigj t1 = sqrt(abs(fff/ar)) t2 = sqrt(abs(fff/cr)) ! if the tilt angle was correct if (t1 .gt. t2) then rmaj = t1 rmin = t2 ! if the tilt angle is not correct, swap the major and minor axes ! and adjust the tilt else rmaj = t2 rmin = t1 tilt = tilt + 90.0d0 co = cosd(tilt) si = sind(tilt) end if ! distance from center to the foci and the eccentricity foci_dist = sqrt(rmaj**2 + rmin**2) eccen = foci_dist/rmaj ! coordinates of the two foci xfoci1 = x0 + co*foci_dist yfoci1 = y0 + si*foci_dist xfoci2 = x0 - co*foci_dist yfoci2 = y0 - si*foci_dist return end subroutine indexx(n,arr,indx) implicit none ! indexes a real array arr(1:n). outputs the array indx(1:n) such ! that arr(indx(j)) is in ascending order for j=1...n. ! input quantities are not changed. ! declare integer n,indx(n),m,nstack parameter (m=7, nstack = 50) integer i,indxt,ir,itemp,j,jstack,k,l,istack(nstack) double precision arr(n),a ! initialize do j=1,n indx(j) = j enddo jstack = 0 l = 1 ir = n ! insertion sort when subbarray small enough 1 if (ir - l .lt. m) then do j=l+1,ir indxt = indx(j) a = arr(indxt) do i=j-1,l,-1 if (arr(indx(i)) .le. a) goto 2 indx(i+1) = indx(i) enddo i = l - 1 2 indx(i+1) = indxt enddo ! pop stack and begin a new round of partitioning if (jstack .eq. 0) return ir = istack(jstack) l = istack(jstack-1) jstack = jstack - 2 ! choose median of left, center and right elements as partitioning element ! also rearrange so that a(l+1) < a(l) < a(ir) else k = (l + ir)/2 itemp = indx(k) indx(k) = indx(l+1) indx(l+1) = itemp if (arr(indx(l)) .gt. arr(indx(ir))) then itemp = indx(l) indx(l) = indx(ir) indx(ir) = itemp end if if(arr(indx(l+1)).gt.arr(indx(ir)))then itemp=indx(l+1) indx(l+1)=indx(ir) indx(ir)=itemp endif if(arr(indx(l)).gt.arr(indx(l+1)))then itemp=indx(l) indx(l)=indx(l+1) indx(l+1)=itemp endif ! initialize pointers for partitioning i = l + 1 j = ir indxt = indx(l+1) a = arr(indxt) 3 continue i = i + 1 if (arr(indx(i)) .lt. a) goto 3 4 continue j = j - 1 if (arr(indx(j)) .gt. a) goto 4 if (j .lt. i) goto 5 itemp = indx(i) indx(i) = indx(j) indx(j) = itemp goto 3 5 indx(l+1) = indx(j) indx(j) = indxt jstack = jstack + 2 ! push pointers to larger subarray on stack if (jstack .gt. nstack) stop 'jstack > nstack in routine indexx' if (ir - i + 1 .ge. j - l) then istack(jstack) = ir istack(jstack-1) = i ir = j - 1 else istack(jstack) = j-1 istack(jstack-1) = l l = i end if end if goto 1 end subroutine gasdev(rvec) implicit none save ! returns two gaussian distributed deviates with zero mean and unit variance ! declare logical gaus_stored double precision harvest,v1,v2,fac,gset,rsq,rvec(2) data gaus_stored /.false./ ! here we go 1 call random_number(rvec) v1 = 2.0d0 * rvec(1) - 1.0d0 v2 = 2.0d0 * rvec(2) - 1.0d0 ! see if they are in the unit circle, if not try again rsq = v1**2 + v2**2 if (rsq .ge. 1.0 .or. rsq .eq. 0.0) goto 1 ! now make the gaussian transform, recipe equation 7.2.10, to get two ! random deviates. return one and save the other for the next time ! then set the flag fac = sqrt(-2.0d0*log(rsq)/rsq) rvec(1) = v1*fac rvec(2) = v2*fac return end