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