1 #! /usr/bin/env python
   2 # -*- coding: utf-8 -*-
   3 
   4 """
   5 http://www.scipy.org/Cookbook/Least_Squares_Circle
   6 """
   7 
   8 from numpy import *
   9 
  10 # Coordinates of the 2D points
  11 
  12 # x = r_[  9, 35, -13,  10,  23,   0]
  13 # y = r_[ 34, 10,   6, -14,  27, -10]
  14 
  15 x = r_[36, 36, 19, 18, 33, 26]
  16 y = r_[14, 10, 28, 31, 18, 26]
  17 
  18 # R0 = 25
  19 # nb_pts = 8
  20 # dR = 2
  21 # angle =9*pi/5
  22 # x = (10 + R0*cos(theta0) + dR*random.normal(size=nb_pts)).round()
  23 # y = (10 + R0*sin(theta0) + dR*random.normal(size=nb_pts)).round()
  24 
  25 
  26 # == METHOD 1 ==
  27 method_1 = 'algebraic'
  28 
  29 # coordinates of the barycenter
  30 x_m = mean(x)
  31 y_m = mean(y)
  32 
  33 # calculation of the reduced coordinates
  34 u = x - x_m
  35 v = y - y_m
  36 
  37 # linear system defining the center in reduced coordinates (uc, vc):
  38 #    Suu * uc +  Suv * vc = (Suuu + Suvv)/2
  39 #    Suv * uc +  Svv * vc = (Suuv + Svvv)/2
  40 
  41 Suv  = sum(u*v)
  42 Suu  = sum(u**2)
  43 Svv  = sum(v**2)
  44 Suuv = sum(u**2 * v)
  45 Suvv = sum(u * v**2)
  46 Suuu = sum(u**3)
  47 Svvv = sum(v**3)
  48 
  49 # Solving the linear system
  50 A = array([ [ Suu, Suv ], [Suv, Svv]])
  51 B = array([ Suuu + Suvv, Svvv + Suuv ])/2.0
  52 uc, vc = linalg.solve(A, B)
  53 
  54 xc_1 = x_m + uc
  55 yc_1 = y_m + vc
  56 
  57 # Calculation of all distances from the center (xc_1, yc_1)
  58 Ri_1     = sqrt((x-xc_1)**2 + (y-yc_1)**2)
  59 R_1      = mean(Ri_1)
  60 residu_1 = sum((Ri_1-R_1)**2)
  61 residu2_1= sum((Ri_1**2-R_1**2)**2)
  62 
  63 # Decorator to count functions calls
  64 import functools
  65 def countcalls(fn):
  66     "decorator function count function calls "
  67 
  68     @functools.wraps(fn)
  69     def wrapped(*args):
  70         wrapped.ncalls +=1
  71         return fn(*args)
  72 
  73     wrapped.ncalls = 0
  74     return wrapped
  75 
  76 #  == METHOD 2 ==
  77 from scipy      import optimize
  78 
  79 method_2  = "leastsq"
  80 
  81 def calc_R(c):
  82     """ calculate the distance of each 2D points from the center c=(xc, yc) """
  83     return sqrt((x-c[0])**2 + (y-c[1])**2)
  84 
  85 @countcalls
  86 def f_2(c):
  87     """ calculate the algebraic distance between the 2D points and the mean circle centered at c=(xc, yc) """
  88     Ri = calc_R(c)
  89     return Ri - Ri.mean()
  90 
  91 center_estimate = x_m, y_m
  92 center_2, ier = optimize.leastsq(f_2, center_estimate)
  93 
  94 xc_2, yc_2 = center_2
  95 Ri_2       = calc_R(center_2)
  96 R_2        = Ri_2.mean()
  97 residu_2   = sum((Ri_2 - R_2)**2)
  98 residu2_2  = sum((Ri_2**2-R_2**2)**2)
  99 ncalls_2   = f_2.ncalls
 100 
 101 # == METHOD 3 ==
 102 from scipy      import  odr
 103 
 104 method_3  = "odr"
 105 
 106 @countcalls
 107 def f_3(beta, x):
 108     """ implicit function of the circle """
 109     xc, yc, r = beta
 110     return (x[0]-xc)**2 + (x[1]-yc)**2 -r**2
 111 
 112 # initial guess for parameters
 113 R_m = calc_R([x_m, y_m]).mean()
 114 beta0 = [ x_m, y_m, R_m]
 115 
 116 # for implicit function :
 117 #       data.x contains both coordinates of the points
 118 #       data.y is the dimensionality of the response
 119 lsc_data  = odr.Data(row_stack([x, y]), y=1)
 120 lsc_model = odr.Model(f_3, implicit=True)
 121 lsc_odr   = odr.ODR(lsc_data, lsc_model, beta0)
 122 lsc_out   = lsc_odr.run()
 123 
 124 xc_3, yc_3, R_3 = lsc_out.beta
 125 Ri_3       = calc_R([xc_3, yc_3])
 126 residu_3   = sum((Ri_3 - R_3)**2)
 127 residu2_3  = sum((Ri_3**2-R_3**2)**2)
 128 ncalls_3   = f_3.ncalls
 129 
 130 print 'lsc_out.sum_square = ',lsc_out.sum_square
 131 
 132 # == METHOD 4 ==
 133 
 134 method_4  = "odr with jacobian"
 135 
 136 @countcalls
 137 def f_4(beta, x):
 138     """ implicit function of the circle """
 139     xc, yc, r = beta
 140     xi, yi    = x
 141 
 142     return (xi-xc)**2 + (yi-yc)**2 -r**2
 143 
 144 @countcalls
 145 def jacb(beta, x):
 146     """ Jacobian function with respect to the parameters beta.
 147     return df/dbeta
 148     """
 149     xc, yc, r = beta
 150     xi, yi    = x
 151 
 152     df_db    = empty((beta.size, x.shape[1]))
 153     df_db[0] =  2*(xc-xi)                     # d_f/dxc
 154     df_db[1] =  2*(yc-yi)                     # d_f/dyc
 155     df_db[2] = -2*r                           # d_f/dr
 156 
 157     return df_db
 158 
 159 @countcalls
 160 def jacd(beta, x):
 161     """ Jacobian function with respect to the input x.
 162     return df/dx
 163     """
 164     xc, yc, r = beta
 165     xi, yi    = x
 166 
 167     df_dx    = empty_like(x)
 168     df_dx[0] =  2*(xi-xc)                     # d_f/dxi
 169     df_dx[1] =  2*(yi-yc)                     # d_f/dyi
 170 
 171     return df_dx
 172 
 173 
 174 def calc_estimate(data):
 175     """ Return a first estimation on the parameter from the data  """
 176     xc0, yc0 = data.x.mean(axis=1)
 177     r0 = sqrt((data.x[0]-xc0)**2 +(data.x[1] -yc0)**2).mean()
 178     return xc0, yc0, r0
 179 
 180 # for implicit function :
 181 #       data.x contains both coordinates of the points
 182 #       data.y is the dimensionality of the response
 183 lsc_data  = odr.Data(row_stack([x, y]), y=1)
 184 lsc_model = odr.Model(f_4, implicit=True, estimate=calc_estimate, fjacd=jacd, fjacb=jacb)
 185 lsc_odr   = odr.ODR(lsc_data, lsc_model)    # beta0 has been replaced by an estimate function
 186 lsc_odr.set_job(deriv=3)                    # use user derivatives function without checking
 187 lsc_out   = lsc_odr.run()
 188 
 189 xc_4, yc_4, R_4 = lsc_out.beta
 190 Ri_4       = calc_R([xc_4, yc_4])
 191 residu_4   = sum((Ri_4 - R_4)**2)
 192 residu2_4  = sum((Ri_4**2-R_4**2)**2)
 193 ncalls_4   = f_4.ncalls
 194 
 195 print "Method 4 :"
 196 print "Functions calls : f_4=%d jacb=%d jacd=%d" % (f_4.ncalls, jacb.ncalls, jacd.ncalls)
 197 
 198 
 199 # Summary
 200 fmt = '%-18s %10.5f %10.5f %10.5f %10d %10.6f %10.6f %10.2f'
 201 print ('\n%-18s' +' %10s'*7) % tuple('METHOD Xc Yc Rc nb_calls std(Ri) residu residu2'.split())
 202 print '-'*(18 +7*(10+1))
 203 print  fmt % (method_1, xc_1, yc_1, R_1,        1, Ri_1.std(), residu_1, residu2_1)
 204 print  fmt % (method_2, xc_2, yc_2, R_2, ncalls_2, Ri_2.std(), residu_2, residu2_2)
 205 print  fmt % (method_3, xc_3, yc_3, R_3, ncalls_3, Ri_3.std(), residu_3, residu2_3)
 206 print  fmt % (method_4, xc_4, yc_4, R_4, ncalls_4, Ri_4.std(), residu_4, residu2_4)
 207 
 208 # plotting functions
 209 
 210 from matplotlib import pyplot as p, cm
 211 
 212 def plot_all(residu2=False, basename='circle'):
 213     """ Draw data points, best fit circles and center for the three methods,
 214     and adds the iso contours corresponding to the fiel residu or residu2
 215     """
 216 
 217     f = p.figure(figsize=(6.5, 4.5), dpi=90, facecolor='white')
 218     p.axis('equal')
 219 
 220     p.plot(x, y, 'ro', label='data', ms=9, mec='b', mew=1)
 221 
 222     theta_fit = linspace(-pi, pi, 180)
 223 
 224     x_fit1 = xc_1 + R_1*cos(theta_fit)
 225     y_fit1 = yc_1 + R_1*sin(theta_fit)
 226     p.plot(x_fit1, y_fit1, 'b-' , label=method_1, lw=2)
 227 
 228     x_fit2 = xc_2 + R_2*cos(theta_fit)
 229     y_fit2 = yc_2 + R_2*sin(theta_fit)
 230     p.plot(x_fit2, y_fit2, 'k--', label=method_2, lw=2)
 231 
 232     x_fit3 = xc_3 + R_3*cos(theta_fit)
 233     y_fit3 = yc_3 + R_3*sin(theta_fit)
 234     p.plot(x_fit3, y_fit3, 'r-.', label=method_3, lw=2)
 235 
 236     p.plot([xc_1], [yc_1], 'bD', mec='y', mew=1)
 237     p.plot([xc_2], [yc_2], 'gD', mec='r', mew=1)
 238     p.plot([xc_3], [yc_3], 'kD', mec='w', mew=1)
 239 
 240     # draw
 241     p.xlabel('x')
 242     p.ylabel('y')
 243     p.legend(loc='best',labelspacing=0.1)
 244 
 245     # plot the residu fields
 246     nb_pts = 100
 247 
 248     p.draw()
 249     xmin, xmax = p.xlim()
 250     ymin, ymax = p.ylim()
 251 
 252     vmin = min(xmin, ymin)
 253     vmax = max(xmax, ymax)
 254 
 255     xg, yg = ogrid[vmin:vmax:nb_pts*1j, vmin:vmax:nb_pts*1j]
 256     xg = xg[..., newaxis]
 257     yg = yg[..., newaxis]
 258 
 259     Rig    = sqrt( (xg - x)**2 + (yg - y)**2 )
 260     Rig_m  = Rig.mean(axis=2)[..., newaxis]
 261 
 262     if residu2 : residu = sum( (Rig**2 - Rig_m**2)**2 ,axis=2)
 263     else       : residu = sum( (Rig-Rig_m)**2 ,axis=2)
 264 
 265     lvl = exp(linspace(log(residu.min()), log(residu.max()), 15))
 266 
 267     p.contourf(xg.flat, yg.flat, residu.T, lvl, alpha=0.75, cmap=cm.Purples_r)
 268     cbar = p.colorbar(format='%.f')
 269 
 270     if residu2 : cbar.set_label('Residu_2')
 271     else       : cbar.set_label('Residu')
 272 
 273     p.xlim(xmin=vmin, xmax=vmax)
 274     p.ylim(ymin=vmin, ymax=vmax)
 275 
 276     p.grid()
 277     p.title('Leasts Squares Circle')
 278     p.savefig('%s_residu%d.png' % (basename, 2 if residu2 else 1))
 279 
 280 plot_all(residu2=False, basename='circle')
 281 plot_all(residu2=True , basename='circle')
 282 
 283 p.show()
 284 # vim: set et sts=4 sw=4: