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 def calc_estimate(data):
 113     """ Return a first estimation on the parameter from the data  """
 114     xc0, yc0 = data.x.mean(axis=1)
 115     r0 = sqrt((data.x[0]-xc0)**2 +(data.x[1] -yc0)**2).mean()
 116     return xc0, yc0, r0
 117 
 118 # for implicit function :
 119 #       data.x contains both coordinates of the points
 120 #       data.y is the dimensionality of the response
 121 lsc_data  = odr.Data(row_stack([x, y]), y=1)
 122 lsc_model = odr.Model(f_3, implicit=True, estimate=calc_estimate)
 123 lsc_odr   = odr.ODR(lsc_data, lsc_model)
 124 lsc_out   = lsc_odr.run()
 125 
 126 xc_3, yc_3, R_3 = lsc_out.beta
 127 Ri_3       = calc_R([xc_3, yc_3])
 128 residu_3   = sum((Ri_3 - R_3)**2)
 129 residu2_3  = sum((Ri_3**2-R_3**2)**2)
 130 ncalls_3   = f_3.ncalls
 131 
 132 print 'lsc_out.sum_square = ',lsc_out.sum_square
 133 
 134 # == METHOD 4 ==
 135 
 136 method_4  = "odr with jacobian"
 137 
 138 @countcalls
 139 def f_4(beta, x):
 140     """ implicit function of the circle """
 141     xc, yc, r = beta
 142     xi, yi    = x
 143 
 144     return (xi-xc)**2 + (yi-yc)**2 -r**2
 145 
 146 @countcalls
 147 def jacb(beta, x):
 148     """ Jacobian function with respect to the parameters beta.
 149     return df/dbeta
 150     """
 151     xc, yc, r = beta
 152     xi, yi    = x
 153 
 154     df_db    = empty((beta.size, x.shape[1]))
 155     df_db[0] =  2*(xc-xi)                     # d_f/dxc
 156     df_db[1] =  2*(yc-yi)                     # d_f/dyc
 157     df_db[2] = -2*r                           # d_f/dr
 158 
 159     return df_db
 160 
 161 @countcalls
 162 def jacd(beta, x):
 163     """ Jacobian function with respect to the input x.
 164     return df/dx
 165     """
 166     xc, yc, r = beta
 167     xi, yi    = x
 168 
 169     df_dx    = empty_like(x)
 170     df_dx[0] =  2*(xi-xc)                     # d_f/dxi
 171     df_dx[1] =  2*(yi-yc)                     # d_f/dyi
 172 
 173     return df_dx
 174 
 175 
 176 def calc_estimate(data):
 177     """ Return a first estimation on the parameter from the data  """
 178     xc0, yc0 = data.x.mean(axis=1)
 179     r0 = sqrt((data.x[0]-xc0)**2 +(data.x[1] -yc0)**2).mean()
 180     return xc0, yc0, r0
 181 
 182 # for implicit function :
 183 #       data.x contains both coordinates of the points
 184 #       data.y is the dimensionality of the response
 185 lsc_data  = odr.Data(row_stack([x, y]), y=1)
 186 lsc_model = odr.Model(f_4, implicit=True, estimate=calc_estimate, fjacd=jacd, fjacb=jacb)
 187 lsc_odr   = odr.ODR(lsc_data, lsc_model)
 188 lsc_odr.set_job(deriv=3)                    # use user derivatives function without checking
 189 lsc_out   = lsc_odr.run()
 190 
 191 xc_4, yc_4, R_4 = lsc_out.beta
 192 Ri_4       = calc_R([xc_4, yc_4])
 193 residu_4   = sum((Ri_4 - R_4)**2)
 194 residu2_4  = sum((Ri_4**2-R_4**2)**2)
 195 ncalls_4   = f_4.ncalls
 196 
 197 print "Method 4 :"
 198 print "Functions calls : f_4=%d jacb=%d jacd=%d" % (f_4.ncalls, jacb.ncalls, jacd.ncalls)
 199 
 200 
 201 # Summary
 202 fmt = '%-18s %10.5f %10.5f %10.5f %10d %10.6f %10.6f %10.2f'
 203 print ('\n%-18s' +' %10s'*7) % tuple('METHOD Xc Yc Rc nb_calls std(Ri) residu residu2'.split())
 204 print '-'*(18 +7*(10+1))
 205 print  fmt % (method_1, xc_1, yc_1, R_1,        1, Ri_1.std(), residu_1, residu2_1)
 206 print  fmt % (method_2, xc_2, yc_2, R_2, ncalls_2, Ri_2.std(), residu_2, residu2_2)
 207 print  fmt % (method_3, xc_3, yc_3, R_3, ncalls_3, Ri_3.std(), residu_3, residu2_3)
 208 print  fmt % (method_4, xc_4, yc_4, R_4, ncalls_4, Ri_4.std(), residu_4, residu2_4)
 209 
 210 # plotting functions
 211 
 212 from matplotlib import pyplot as p, cm
 213 
 214 def plot_all(residu2=False, basename='circle'):
 215     """ Draw data points, best fit circles and center for the three methods,
 216     and adds the iso contours corresponding to the fiel residu or residu2
 217     """
 218 
 219     f = p.figure(figsize=(6.5, 4.5), dpi=90, facecolor='white')
 220     p.axis('equal')
 221 
 222     p.plot(x, y, 'ro', label='data', ms=9, mec='b', mew=1)
 223 
 224     theta_fit = linspace(-pi, pi, 180)
 225 
 226     x_fit1 = xc_1 + R_1*cos(theta_fit)
 227     y_fit1 = yc_1 + R_1*sin(theta_fit)
 228     p.plot(x_fit1, y_fit1, 'b-' , label=method_1, lw=2)
 229 
 230     x_fit2 = xc_2 + R_2*cos(theta_fit)
 231     y_fit2 = yc_2 + R_2*sin(theta_fit)
 232     p.plot(x_fit2, y_fit2, 'k--', label=method_2, lw=2)
 233 
 234     x_fit3 = xc_3 + R_3*cos(theta_fit)
 235     y_fit3 = yc_3 + R_3*sin(theta_fit)
 236     p.plot(x_fit3, y_fit3, 'r-.', label=method_3, lw=2)
 237 
 238     p.plot([xc_1], [yc_1], 'bD', mec='y', mew=1)
 239     p.plot([xc_2], [yc_2], 'gD', mec='r', mew=1)
 240     p.plot([xc_3], [yc_3], 'kD', mec='w', mew=1)
 241 
 242     # draw
 243     p.xlabel('x')
 244     p.ylabel('y')
 245     p.legend(loc='best',labelspacing=0.1)
 246 
 247     # plot the residu fields
 248     nb_pts = 100
 249 
 250     p.draw()
 251     xmin, xmax = p.xlim()
 252     ymin, ymax = p.ylim()
 253 
 254     vmin = min(xmin, ymin)
 255     vmax = max(xmax, ymax)
 256 
 257     xg, yg = ogrid[vmin:vmax:nb_pts*1j, vmin:vmax:nb_pts*1j]
 258     xg = xg[..., newaxis]
 259     yg = yg[..., newaxis]
 260 
 261     Rig    = sqrt( (xg - x)**2 + (yg - y)**2 )
 262     Rig_m  = Rig.mean(axis=2)[..., newaxis]
 263 
 264     if residu2 : residu = sum( (Rig**2 - Rig_m**2)**2 ,axis=2)
 265     else       : residu = sum( (Rig-Rig_m)**2 ,axis=2)
 266 
 267     lvl = exp(linspace(log(residu.min()), log(residu.max()), 15))
 268 
 269     p.contourf(xg.flat, yg.flat, residu.T, lvl, alpha=0.75, cmap=cm.Purples_r)
 270     cbar = p.colorbar(format='%.f')
 271 
 272     if residu2 : cbar.set_label('Residu_2')
 273     else       : cbar.set_label('Residu')
 274 
 275     p.xlim(xmin=vmin, xmax=vmax)
 276     p.ylim(ymin=vmin, ymax=vmax)
 277 
 278     p.grid()
 279     p.title('Leasts Squares Circle')
 280     p.savefig('%s_residu%d.png' % (basename, 2 if residu2 else 1))
 281 
 282 plot_all(residu2=False, basename='circle')
 283 plot_all(residu2=True , basename='circle')
 284 
 285 p.show()
 286 # vim: set et sts=4 sw=4: