better but not yet perfect autofocus

Readme.md

@@ -320,9 +320,23 @@ X10193.88 Y-8892
 
 -> z has wrong sign !
 
+----------------
+avoid constand RBV event:
+caput SAR-ESPMX:MOT_FX 0.001
+caput SAR-ESPMX:MOT_FY 0.001
+---------- TODO ----------
+-faster autofocus:
+   - move at constant speed and acquire as fast as possible instead stop and go motion
+
+- find fiducials:
+   - fix openCV version problems
+
+- configuration: use the current config as default values
+
+- automation: towards automatic alignement
+
+
 
 
 
-!!!!!!!!!!!  AFTER COLLECTION GRAPHICAL OVERLAY OBJECTS ARE SHIFTED !!!!!!!!!!!
-!!!!!!!!!!! IF MOVING ON THE EPICS PANEL, THE OBJECTS GET OUT OF SYNC !!!!!!!!
 

geometry.py

@@ -255,84 +255,6 @@ class geometry:
 
     _log.debug('least square data:\nK:{}\nAA:{}'.format(K, AA))
 
-  @staticmethod
-  def autofocus(cam,mot,rng=(-1,1),n=30,progressDlg=None,saveImg=False):
-    # cam   camera object
-    # mot   motor object (e.g. SimMotorTweak)
-    # rng   region (min max relative to current position) to seek
-    # n     number of images to take in region
-    # progressDlg a pg.ProgressDialog to allow to abort the function call
-
-    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()
-        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()}')
-        mot.move_abs(p0)
-        return p0
-    else:
-      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.get_image()  # get latest 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()
-        try:
-          progressDlg+=1
-          if progressDlg.wasCanceled():
-            return
-        except TypeError as e:
-          pass
-        _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>1 and mx+2<=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
-
   @staticmethod
   def find_fiducial(cam,sz=(210,210),brd=(20,20)):
     if type(cam)==str:
@@ -340,6 +262,8 @@ class geometry:
       img=np.asarray(img)
     else:
       img=cam.get_image()  # get latest image
+      if img.dtype==np.uint16:
+        img=np.array(img,np.uint8)
 
     fid=np.ones((sz[1]+2*brd[1],sz[0]+2*brd[0]),dtype=np.uint8)*255
     fid[brd[1]:sz[1]+brd[1],brd[0]:sz[0]+brd[0]]=0
@@ -481,6 +405,169 @@ class geometry:
     self._fitPlane=aa
 
 
+class autofocus:
+
+  def __init__(self,cam,mot=None):
+    # cam   camera object or image list
+    # mot   motor object (e.g. SimMotorTweak) or None
+    # rng   region (min max relative to current position) to seek
+    self._cam=cam
+    self._mot=mot
+
+  def save_img(self,rng=(-1,1),n=30,progressDlg=None):
+    mot=self._mot
+    cam=self._cam
+    try:
+      p0=mot.get_rbv()
+    except AttributeError:
+      _log.warning('needs a real motor')
+      return
+
+    for i,p in enumerate(np.linspace(p0+rng[0],p0+rng[1],n)):
+      mot.move_abs(p,wait=True)
+      try:
+        pic=cam._pic
+      except AttributeError:
+        pic=cam.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()}')
+      mot.move_abs(p0)
+
+  def cb_autofocus(self,*args,**kwargs):
+    cam=self._cam
+    mot=self._mot
+    mtr=self._mtr
+    msk=self._msk
+    idx=self._idx
+    p=mot.get_rbv()
+    img16=np.array(cam._pic, 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)
+    m=sb.sum()
+    mtr[idx,:]=(p,m)
+    _log.debug(f'{args} {kwargs} p:{p} mtr={m}')
+    print(p)
+    if abs(p-self._pDst)<0.01:
+      _log.debug(f'DONE->{self._idx}')
+      del cam.process
+      print(mtr[:idx,:])
+      mx=mtr[:,1].argmax()
+      self._af=self.interpolate(mtr[:idx,0], mtr[:idx,1])
+      return
+    self._idx+=1
+
+
+
+    #if p==mot.move_abs(p0+rng[1], wait=False)
+    # move to sharp location
+    # and unregister callback
+
+  def run_continous(self,rng=(-1,1),velo=0.5):
+    #do not stop and go, use ballback for processing
+    mot=self._mot
+    cam=self._cam
+    p0=mot.get_rbv()
+    v=mot._motor.get('VELO')
+    p0=mot.get_rbv()
+    mot._motor.put('VELO',2)
+    mot.move_abs(p0+rng[0], wait=True)
+    mot._motor.put('VELO',velo)
+
+
+    n=int((rng[1]-rng[0])/velo)*20 # assume max 20fps
+    self._mtr=mtr=np.ndarray(shape=(n,2))
+    self._idx=0
+    self._pDst=p0+rng[1]
+    self._msk=np.array(((1, 0, -1), (2, 0, -2), (1, 0, -1)), np.int16)
+    cam.process=self.cb_autofocus
+    mot.move_abs(p0+rng[1], wait=False)
+
+    import pyqtgraph as pg
+    import time
+    with pg.ProgressDialog('Progress', 0, n) as dlg:
+      while True:
+        try:
+          p=self._af
+        except AttributeError:
+          time.sleep(.1)
+          dlg.setValue(self._idx)
+          continue
+        break
+    _log.debug(f' Final AF:{p}')
+    mot._motor.put('VELO',2)
+    mot.move_abs(p, wait=False)
+
+  @staticmethod
+  def interpolate(posLst,mtr):
+    mx=mtr[:].argmax()
+    _log.debug(f'best focus at idx:{mx}= pos:{posLst[mx]} = metric:{mtr[mx]:.6g}')
+    if mx>1 and mx+2<=mtr.shape[0]:
+      #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]
+    return p
+
+  def run(self,rng=(-1,1),n=30,progressDlg=None,saveImg=False):
+    # rng   region (min max relative to current position) to seek
+    # n     number of images to take in region
+    # progressDlg a pg.ProgressDialog to allow to abort the function call
+    mot=self._mot
+    cam=self._cam
+    if mot is not None:
+      p0=mot.get_rbv()
+    else:
+      p0=0
+    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.get_image()  # get latest 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()
+      try:
+        progressDlg+=1
+        if progressDlg.wasCanceled():
+          return
+      except TypeError as e:
+        pass
+      _log.debug(f'{i}/{p:.4g} ->  {mtr[i]:.4g}')
+    p=autofocus.interpolate(posLst,mtr)
+    if mot is not None:
+      mot.move_abs(p)
+    return p
+
 if __name__=="__main__":
   import argparse
 
@@ -676,7 +763,8 @@ if __name__=="__main__":
   if args.mode&0x08:
     import glob
     imgLst=sorted(glob.glob("scratch/autofocus2/image*.png"))
-    geometry.autofocus(imgLst,None)
+    af=autofocus(imgLst,None)
+    af.run()
   if args.mode&0x10:
     geometry.find_fiducial("scratch/fiducial/image001.png")
 

swissmx.py

@@ -666,6 +666,7 @@ class WndSwissMx(QMainWindow, Ui_MainWindow):
   def cb_new_frame_pv(self, **kwargs):
     #thrd=threading.current_thread()
     #_log.debug(f'thread:{thrd.getName()}, {thrd.native_id}')
+    #_log.debug(f"{kwargs['timestamp']}")
 
     app=QApplication.instance()
     cam=app._camera
@@ -679,6 +680,11 @@ class WndSwissMx(QMainWindow, Ui_MainWindow):
       pic.dtype=np.uint16
     camera.epics_cam.set_fiducial(pic, 255)
     cam._pic=pic
+    cam._timestamp=kwargs['timestamp']
+    try:
+      cam.process()
+    except AttributeError as e:
+      pass
     # self._goImg.setImage(cam._pic) caused some deadlocks.
     # therefore try to update the image with signals instead
     self.sigNewCamImg.emit()
@@ -1613,14 +1619,39 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
       af_range=misc['af_range']
       n=misc['af_steps']
       rng=(-af_range/2, af_range/2)
-      with pg.ProgressDialog('Progress', 0, n) as dlg:
+      #geo.autofocus(app._camera, self.tweakers['base_z'],rng, n,dlg)
-        geo.autofocus(app._camera, self.tweakers['base_z'],rng, n,dlg)
+      af= geometry.autofocus(app._camera, self.tweakers['base_z'])
+      #  af.run(rng, n, dlg)
+
+      #with pg.ProgressDialog('Progress', 0, n) as dlg:
+      af.run_continous(rng=(-.5, .5), velo=0.3)
 
   def cb_find_fiducial(self):
       app=QApplication.instance()
       geo=app._geometry
       #geo.autofocus(app._camera, self.tweakers['base_z'],rng=(-1, 1), n=30,saveImg=True)
       pos,corr=geo.find_fiducial(app._camera, sz=(210,210),brd=(20,20))
+
+      fx=self.tweakers["fast_x"].get_rbv()
+      fy=self.tweakers["fast_y"].get_rbv()
+      bz=self.tweakers["base_z"].get_rbv()
+
+      #oc=geo._opt_ctr
+      #bm_pos=self._goBeamMarker.pos()
+      #bm_sz=self._goBeamMarker.size()
+      #pos_eu=geo.pix2pos(pos)
+      #pImg=pg.Point(self._goImg.mapFromScene(pos))
+      #pTrk=pg.Point(self._goTracked.mapFromScene(pos))
+      pos_scn=self._goImg.mapToScene(pg.Point(pos))
+      pos_eu=self._goTracked.mapFromScene(pos_scn)
+
+      l=.120
+      #go=UsrGO.Fiducial((fx-l/2, fy-l/2), (l, l), bz)
+      go=UsrGO.Fiducial((pos_eu.x()-l/2, pos_eu.y()-l/2), (l, l), bz)
+      go.sigRegionChangeFinished.connect(self.cb_fiducial_update_z)
+      grp=self._goTracked
+      grp.addItem(go)
+      self._moduleFixTarget._tree.setData(grp.childItems())
       pass
 
   def cb_testcode(self):

toolbox/Readme.md

@@ -0,0 +1,4 @@
+toolbox
+--------
+
+this directory contains test applications to analyse data and try basic functionalities.

toolbox/fiducialDetection.py

@@ -34,7 +34,8 @@ def set_fiducial(pic):
   pic[0:6, 0:5]=f*pic.max()
 
 def wtestimages():
-  fn=os.path.expanduser('~/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/SwissMX/scratch/fiducial/image{idx:03d}.png')
+  #fn=os.path.expanduser('~/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/SwissMX/scratch/fiducial/image{idx:03d}.png')
+  fn=os.path.expanduser('../scratch/fiducial/image{idx:03d}.png')
   img=np.kron(np.array([[1, 0] * 5, [0, 1] * 5] * 6,dtype=np.uint8), np.ones((100, 100),dtype=np.uint8)*255)
   set_fiducial(img)
   pil_img=PIL.Image.fromarray(img)
@@ -52,7 +53,7 @@ def wtestimages():
 
   img=np.ndarray((1200,1000))
 
-wtestimages()
+#wtestimages()
 
 
 class fiducial(QtGui.QMainWindow):

workbench/Readme.md

@@ -1,8 +0,0 @@
-testApps
---------
-
-this directory contains test applications to analyse data and try basig functionalities.
-
-'scratch' contains quick and dirty basic tests
-
-'testApps' contains clean small applications to tests e.g. image analysis stuff.