towards fiducial detection

geometry.py

@@ -248,7 +248,7 @@ class geometry:
     _log.debug('least square data:\nK:{}\nAA:{}'.format(K, AA))
 
   @staticmethod
-  def autofocus(cam,mot,rng=(-1,1),n=30):
+  def autofocus(cam,mot,rng=(-1,1),n=30,saveImg=False):
     # cam   camera object
     # mot   motor object
     # rng   region (min max relative to current position) to seek
@@ -256,33 +256,11 @@ class geometry:
     # roi   region of interrest to calculate sharpness
     # mode  mode to calculate sharpness (sum/max-min/hist?  of edge detection in roi)
     import PIL.Image
-    from scipy import ndimage
-    v=np.ndarray(shape=(len(cam),2))
-    if type(cam) == list:
-      for i, fn in enumerate(cam):
-        img=PIL.Image.open(fn)
-        img=np.asarray(img)
-        s=ndimage.sobel(img)
-        v[i,0]=s.sum()
-        v[i,1]=s.std()
-        #fft=np.log(np.abs(np.fft.fft2(img)))
-        #fft=np.fft.fftshift(fft)
-        #s=np.array(fft.shape,dtype=np.uint16)/2
-        #fft[300:700,400:800]=0
-        #v[i,1]=fft.sum()
-        #if i&0x3==0:
-        #  plt.figure()
-        #  plt.imshow(fft)
-      fig, ax=plt.subplots()
-      mx=v.max(0)
-      mn=v.min(0)
-      v=(v-mn)/(mx-mn)
-      #ax.plot(v[:,0])
-      ax.plot(v)
-      plt.show()
-      pass
-    else:
+    if mot is not None:
       p0=mot.get_rbv()
+    else:
+      p0=0
+    if saveImg:
       for i,p in enumerate(np.linspace(p0+rng[0],p0+rng[1],n)):
         mot.move_abs(p,wait=True)
         pic=cam._pic# get_image()
@@ -292,11 +270,55 @@ class geometry:
           pic=np.array(pic/scl, dtype=np.uint8)
         elif pic.dtype!=np.uint8:
           pic=np.array(pic, dtype=np.uint8)
-
         img=PIL.Image.fromarray(pic)
         fn=f'/tmp/image{i:03d}.png'
         img.save(fn)
         _log.debug(f'{fn} {pic.dtype} {pic.min()} {pic.max()}')
+        mot.move_abs(p0)
+        return p0
+    else:
+      from scipy import ndimage, signal
+      if type(cam) == list:
+        imgLst=cam
+        n=len(imgLst)
+      mtr=np.ndarray(shape=(n,))
+      posLst=np.linspace(p0+rng[0], p0+rng[1], n)
+      for i,p in enumerate(posLst):
+        if type(cam)==list:
+          img=PIL.Image.open(imgLst[i])
+          img=np.asarray(img)
+        else:
+          mot.move_abs(p, wait=True)
+          img=cam._pic  # get_image()
+        img16=np.array(img, np.int16)
+        msk=np.array(((1, 0, -1), (2, 0, -2), (1, 0, -1)), np.int16)
+        sb1=signal.convolve2d(img16, msk, mode='same', boundary='fill', fillvalue=0)
+        sb2=signal.convolve2d(img16, msk.T, mode='same', boundary='fill', fillvalue=0)
+        sb=np.abs(sb1)+np.abs(sb2)
+        mtr[i]=sb.sum()
+        _log.debug(f'{i}/{p:.4g} ->  {mtr[i]:.4g}')
+
+      mx=mtr.argmax()
+      _log.debug(f'best focus at idx:{mx}= pos:{posLst[mx]} = metric:{mtr[mx]:.6g}')
+      if mx>0 and mx <len(posLst):
+        #fit parabola and interpolate:
+        # y=ax2+bx+c, at positions x=-1, 0, 1, y'= 2a+b == 0 (top of parabola)
+        # calc a,b,c:
+        # y(-1)=a-b+c
+        # y( 0)=   +c
+        # y( 1)=a+b+c
+        # c=y(0)
+        # b=(y(1)-y(-1))/2
+        # a=(y(1)+y(-1))/2-y(0)
+        # x=-b/2a=(y(-1)-y(1))/2(y(-1)+y(1)-2y(0))
+        u,v,w=mtr[mx-1:mx+2]
+        d=posLst[1]-posLst[0]
+        p=posLst[mx]+d*.5*(u-w)/(u+w-2*v)
+      else:
+        p=posLst[mx]
+      if mot is not None:
+        mot.move_abs(p)
+      return p
     pass
 
   def pix2pos(self, p, zoom=None):
@@ -603,7 +625,7 @@ if __name__=="__main__":
 
   if args.mode&0x08:
     import glob
-    imgLst=sorted(glob.glob("scratch/image*.png"))
+    imgLst=sorted(glob.glob("scratch/autofocus2/image*.png"))
     geometry.autofocus(imgLst,None)