From c3542ad38cac7d621692051c5f022492b426d7f2 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Mon, 19 Sep 2022 17:20:00 +0200 Subject: [PATCH] better but not yet perfect autofocus --- Readme.md | 18 +- geometry.py | 246 +++++++++++++------- swissmx.py | 35 ++- toolbox/Readme.md | 4 + {workbench => toolbox}/autofocus.py | 0 {workbench => toolbox}/fiducialDetection.py | 5 +- workbench/Readme.md | 8 - 7 files changed, 223 insertions(+), 93 deletions(-) create mode 100644 toolbox/Readme.md rename {workbench => toolbox}/autofocus.py (100%) rename {workbench => toolbox}/fiducialDetection.py (96%) delete mode 100644 workbench/Readme.md diff --git a/Readme.md b/Readme.md index d3e8c32..047469a 100644 --- a/Readme.md +++ b/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 !!!!!!!! diff --git a/geometry.py b/geometry.py index 0872f97..7d5e805 100755 --- a/geometry.py +++ b/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") diff --git a/swissmx.py b/swissmx.py index d2bfde9..e572256 100755 --- a/swissmx.py +++ b/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): diff --git a/toolbox/Readme.md b/toolbox/Readme.md new file mode 100644 index 0000000..ec222de --- /dev/null +++ b/toolbox/Readme.md @@ -0,0 +1,4 @@ +toolbox +-------- + +this directory contains test applications to analyse data and try basic functionalities. diff --git a/workbench/autofocus.py b/toolbox/autofocus.py similarity index 100% rename from workbench/autofocus.py rename to toolbox/autofocus.py diff --git a/workbench/fiducialDetection.py b/toolbox/fiducialDetection.py similarity index 96% rename from workbench/fiducialDetection.py rename to toolbox/fiducialDetection.py index f9e8e4e..99ca1b3 100644 --- a/workbench/fiducialDetection.py +++ b/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): diff --git a/workbench/Readme.md b/workbench/Readme.md deleted file mode 100644 index 32c60c2..0000000 --- a/workbench/Readme.md +++ /dev/null @@ -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. \ No newline at end of file