add workbench autofocus.py

geometry.py

@@ -247,13 +247,56 @@ class geometry:
 
     _log.debug('least square data:\nK:{}\nAA:{}'.format(K, AA))
 
-  def autofocus(self):
+  @staticmethod
+  def autofocus(cam,mot,rng=(-1,1),n=30):
     # cam   camera object
     # mot   motor object
     # rng   region (min max relative to current position) to seek
     # n     number of images to take in region
     # 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:
+      p0=mot.get_rbv()
+      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()
+        mx=pic.max()
+        if pic.max()>255:
+          scl=2**int(round(np.log2(mx)-8))
+          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()}')
     pass
 
   def pix2pos(self, p, zoom=None):
@@ -558,6 +601,11 @@ if __name__=="__main__":
              [2.84446241, 3.54734879, 3.1415]])
       plane=geometry.least_square_plane(pts)
 
+  if args.mode&0x08:
+    import glob
+    imgLst=sorted(glob.glob("scratch/image*.png"))
+    geometry.autofocus(imgLst,None)
+
 
 # pix2pos="[[1.0, 200.0, 400.0, 600.0, 800.0], [[[0.001182928623952055, -2.6941995127711305e-05], [-4.043716694634124e-05, -0.0011894314084263825]], [[0.0007955995220142541, -3.175003727901119e-05], [-2.0896601103372113e-05, -0.0008100805094631365]], [[0.00048302539335378367, -1.1661121407652543e-05], [-2.0673746995751222e-05, -0.0004950857899461772]], [[0.00028775903460576395, -1.3762555219494508e-05], [-9.319936861519268e-06, -0.0002889214488565999]], [[0.0001788819256630411, -6.470841493681516e-06], [-2.0336605088889967e-06, -0.0001831131753499113]]]]"