""" Library functions for speckle feature tracking investigations """ import numpy as np import matplotlib.pyplot as plt plt.ion() import time import cv2 as cv #%%----------------------------------------------------------------------------- # Rigid transform calculations with/without centre of rotation calculation #------------------------------------------------------------------------------- def get_transform( pointsA, pointsB ): """ Calculate the transformation between two frames from a series of points identified in each. returns dx,dy, theta """ if len(pointsB)!=len(pointsA): raise Exception('Expected equal number of points A and B') # 1) Find the sums needed for least squares solution # S1 = sum( xi**2 + yi**2 ) # S2 = sum( xi ) # S3 = sum ( yi ) # NC = number of points # Sk1 = sum( wi*(xi*kxi + yi*kyi) ) # Sk2 = sum( wi*(xi*kyi - yi*kxi) ) # Sk3 = sum( wi*(kxi) ) # Sk4 = sum( wi*(kyi) ) S1 = S2 = S3 = 0 Sk1 = Sk2 = Sk3 = Sk4 = 0 NC = len(pointsA) for i in xrange(0,NC): x,y = pointsA[i] S1 += (x**2 + y**2) S2 += x S3 += y kx,ky = pointsB[i] Sk1 += (x*kx + y*ky) Sk2 += (x*ky - y*kx) Sk3 += kx Sk4 += ky # 2) calculate some other values used in muliple times K1 = (NC*S1-S2**2-S3**2) denom = 1.0/K1 K2 = (S2**2 + S3**2 + K1)/S1 #X = denom * Matrix([ #X = denom * sp.Matrix([ # [( -S2*Sk3 - S3*Sk4 + Sk1*K2 ) ], # [( -S2*Sk4 + S3*Sk3 + Sk2*K2 ) ], # [ S1*Sk3 - S2*Sk1 + S3*Sk2 ], # [ S1*Sk4 - S2*Sk2 - S3*Sk1 ]]) #where: # X = sp.Matrix( [cos, sin, dx, dy]) cos_theta = denom*( -S2*Sk3 - S3*Sk4 + Sk1*K2 ) sin_theta = denom*( -S2*Sk4 + S3*Sk3 + Sk2*K2 ) dx = denom*( S1*Sk3 - S2*Sk1 + S3*Sk2 ) dy = denom*(S1*Sk4 - S2*Sk2 - S3*Sk1 ) theta = np.degrees(np.arctan2(sin_theta,cos_theta)) return dx, dy, theta def get_transform2( pointsA, pointsB ): """ Calculate the transformation between two frames from a series of points identified in each. Also calculates the centre of rotation. returns dx,dy, theta , Cx,Cy """ if len(pointsB)!=len(pointsA): raise Exception('Expected equal number of points A and B') # 1) Find the sums needed for least squares solution # S1 = sum( xi**2 + yi**2 ) # S2 = sum( xi ) # S3 = sum ( yi ) # NC = number of points # Sk1 = sum( wi*(xi*kxi + yi*kyi) ) # Sk2 = sum( wi*(xi*kyi - yi*kxi) ) # Sk3 = sum( wi*(kxi) ) # Sk4 = sum( wi*(kyi) ) S1 = S2 = S3 = 0 Sk1 = Sk2 = Sk3 = Sk4 = 0 NC = len(pointsA) for i in xrange(0,NC): x,y = pointsA[i] S1 += (x**2 + y**2) S2 += x S3 += y kx,ky = pointsB[i] Sk1 += (x*kx + y*ky) Sk2 += (x*ky - y*kx) Sk3 += kx Sk4 += ky # 2) calculate some other values used in muliple times K1 = (NC*S1-S2**2-S3**2) denom = 1/K1 K2 = (S2**2 + S3**2 + K1)/S1 #X = denom * Matrix([ #X = denom * sp.Matrix([ # [( -S2*Sk3 - S3*Sk4 + Sk1*K2 ) ], # [( -S2*Sk4 + S3*Sk3 + Sk2*K2 ) ], # [ S1*Sk3 - S2*Sk1 + S3*Sk2 ], # [ S1*Sk4 - S2*Sk2 - S3*Sk1 ]]) #where: # X = sp.Matrix( [cos, sin, dx, dy]) cos_theta = denom*( -S2*Sk3 - S3*Sk4 + Sk1*K2 ) sin_theta = denom*( -S2*Sk4 + S3*Sk3 + Sk2*K2 ) dx = denom*(S1*Sk3 - S2*Sk1 + S3*Sk2 ) #need negative here to correct for dy = denom*(S1*Sk4 - S2*Sk2 - S3*Sk1 ) #... image coords. theta_r = np.arctan2(sin_theta,cos_theta) theta = np.degrees(theta_r) # 3) find centre of rotation #C = sp.Matrix( ('Cx','Cy')) #T = sp.Matrix( ('dx','dy')) #I = sp.Matrix( ((1,0),(0,1)) ) #theta = sp.symbols('theta') #R = sp.Matrix( ( (sp.cos(theta) , -sp.sin(theta)),(sp.sin(theta), sp.cos(theta)) ) ) #From at C=(Cx,Cy) then total translation is just from T #C = RC + T #C-RC = T #C(I-R) = T #C = (I-R).inv() * T if cos_theta == 1: Cx = None Cy = None else: #recalculate cos_theta/sin_theta for better stability cos_theta = np.cos(-theta_r) sin_theta = np.sin(-theta_r) #find centre of rotation Cx = dx*( 1/(-cos_theta + 1) - sin_theta**2 /(2*(-cos_theta + 1)**2) ) \ -dy*sin_theta/(2*(-cos_theta + 1)) Cy = (dx*sin_theta)/(2*(-cos_theta + 1)) + dy/2 #translate (0,0) to (Cx,Cy) frame x0 = -Cx y0 = -Cy print 'x0,y0 : ',(x0,y0) #find shifted (0,0) point in (Cx,Cy) frame x1 = cos_theta*x0 - sin_theta*y0 y1 = sin_theta*x0 + cos_theta*y0 print 'x1,y1 : ',(x1,y1) #find shift at (0,0) due to theta dx_rot = x1-x0 dy_rot = y1-y0 #correct dx,dy print 'dx,dy',(dx,dy) print 'dx,dy rot: ',(dx_rot, dy_rot) dx = dx - dx_rot dy = dy - dy_rot print (dx,dy) return dx, dy, theta, Cx,Cy #%%----------------------------------------------------------------------------- # Pure python cross-correlation functions #------------------------------------------------------------------------------- def normxcorr_circ(f, t): """ Normalised circular cross-correlation """ #subtract mean from t t = t - t.mean() #find sum of squares of t sum_t2 = (t*t).sum() #find sum of squares of f N = f.size sum_f = f.sum() #sum of image values sum_f2 = (f*f).sum() #sum of squares of image values #sum_f = float(sum_f) #sum_f2 = float(sum_f2) #find normalisation factor normf = np.sqrt( (sum_f2 - (sum_f*sum_f)/N) ) norm = 1.0/( ( normf * np.sqrt(sum_t2) ) ) #calc cc F = np.fft.rfft2(f) T = np.fft.rfft2(t) CC = (F*T.conj())* norm cc = np.fft.irfft2(CC) cc = np.fft.fftshift(cc) return cc def find_peak_gaussian_circ(cc): """ Find the peak using gaussian subpixel interpolation corrected for circular correlation cc - correlation """ #find peak and qvalue p = cc.flatten().argmax() py = p//cc.shape[1] px = p%cc.shape[1] mean = abs(cc).mean() q = cc[py,px]/mean ##find the peak value and weighting ny,nx = cc.shape x0 = y0 = cc[py,px] w0 = ( (nx -abs(px - nx//2)) * (ny -abs(py - ny//2)) ) ##Do y fitting if py-1<0:#wrap around ym1 = cc[ny-1, px ] else: ym1 = cc[py-1, px ] wm1 = ( (nx -abs(px - nx//2)) * (ny -abs(py-1 - ny//2)) ) if py+1 >= cc.shape[0]: #wrap arround yp1 = cc[ 0, px ] else: yp1 = cc[py+1, px] wp1 = ( (nx -abs(px - nx//2)) * (ny -abs(py+1 - ny//2)) ) #subtract mean before weighting y0 = (y0-mean) ym1 = (ym1-mean)*(float(w0)/wm1); yp1 = (yp1-mean)*(float(w0)/wp1); top = ( np.log(ym1) -np.log(yp1) ) bot = (2*(np.log(ym1) + np.log(yp1) - 2*np.log(y0) )) if bot==0: sy = py else: sy = py +( top / bot) ##Do x fitting if px-1<0:#wrap around xm1 = cc[ py, nx-1 ] else: xm1 = cc[ py, px-1 ] wm1 = ( (nx -abs(px-1 - nx//2)) * (ny -abs(py - ny//2)) ) if px+1>=cc.shape[1]:#wrap around xp1 = cc[ py, 0 ] else: xp1 = cc[ py, px+1 ] wp1 = ( (nx -abs(px+1 - nx//2)) * (ny -abs(py - ny//2)) ) #subtract mean before weighting x0 = (x0-mean) xm1 = (xm1-mean)*(float(w0)/wm1); xp1 = (xp1-mean)*(float(w0)/wp1); top = ( np.log(xm1) -np.log(xp1) ) bot = (2*(np.log(xm1) + np.log(xp1) - 2*np.log(x0) )) if bot==0: sx=px else: sx = px +( top/bot) if np.isnan(sy): sy = py #print 'nan in peak fitting' if np.isnan(sx): sx = px #print 'nan in peak fitting' return sy,sx,q #%%----------------------------------------------------------------------------- # Modelling translations/rotations #------------------------------------------------------------------------------- def get_shifted_windows(ref_img, win0=(0,0,128,128), delta=(0,0), M=10): """ Return a shifted window in the image for subpixel shifts in 1/M px steps win0 =(x0,y0,w,h) must allow for 1px border in calculations delta = (dx, dy) in pixels """ m=1.0/M #initial window location x0,y0,w0,h0 = win0 img0 = ref_img[y0:y0+h0, x0:x0+w0] #split shift into whole pixels and subpixels delta = np.rint(np.array(delta)*M) #print delta dx = (delta[0])//M #whole pixels dy = (delta[1])//M sdx = (delta[0])%M #sub-pixels sdy = (delta[1])%M #print 'dx,dy,sdx,sdy', dx,dy,sdx,sdy #find shifted window location to whole pixels #scale ref_img up using nearest neighbour - only for a region around the new #shifted window (a 1px border around window to account for subpixel part of shift) x0 = x0+dx -1 y0 = y0+dy -1 x1 = x0+w0+2 y1 = y0+h0+2 #print 'x0,y0,x1,y1', x0,y0,x1,y1 # cv.INTER_NEAREST # cv.INTER_LINEAR # cv.INTER_AREA ref_img_M = cv.resize(ref_img[y0:y1, x0:x1], dsize=None, fx=M, fy=M, interpolation=cv.INTER_NEAREST) #find the subpixel shifted window inside the magnified image Mx0 = M + sdx #window starts M pixels in now, then shift by the subpixels amount My0 = M + sdy Mx1 = Mx0 + (w0*M) My1 = My0 + (h0*M) #print 'Mx0,My0, Mx1,My1', Mx0,My0,Mx1,My1 #find the new window location img1_M = ref_img_M[ My0:My1, Mx0:Mx1] #scale the shifted image down img1 = cv.resize(img1_M, dsize=None, fx=m, fy=m, interpolation=cv.INTER_AREA) return np.ascontiguousarray(img0), np.ascontiguousarray(img1) def get_rotated_window(ref_img, shape=(256,256), ang=0.0): """ Return a rotated window in the reference speckle pattern. returns img0 (un-rotated) and img1 (rotated) """ w,h = shape rows, cols = ref_img.shape M = cv.getRotationMatrix2D((cols/2,rows/2),ang,1) imgR = cv.warpAffine(ref_img,M,(cols,rows), flags=cv.INTER_CUBIC, borderMode=cv.BORDER_CONSTANT, borderValue=0) x0 = (cols/2) - (w/2) y0 = (rows/2) - (h/2) img0 = ref_img[y0:y0+h,x0:x0+w] img1 = imgR[y0:y0+h,x0:x0+w] return np.ascontiguousarray(img0), np.ascontiguousarray(img1) #%%----------------------------------------------------------------------------- # Matcher method classes to allow easier to read code and store data internally #------------------------------------------------------------------------------- class Method(): def __init__(self, name=''): self.method = name #store data internally self.Axs = [] self.Ays = [] self.thetas = [] self.N0s = [] self.N1s = [] self.Nms = [] self.Nfms = [] def reset(self): self.Axs = [] self.Ays = [] self.thetas = [] self.N0s = [] self.N1s = [] self.Nms = [] self.Nfms = [] class NCC(Method): def __init__(self): """ Normalised circular correlation method. No rotation calc. q-values stored in self.NOs """ Method.__init__(self, 'circular normalised cross-correlation') def calc_shift(self, img0,img1): """ Find the translation between frames using the circular normaliued cross-correlation """ #correlate cc = normxcorr_circ(img0,img1) #find peak py,px,q = find_peak_gaussian_circ(cc) Ax = (px-(cc.shape[0]//2)) Ay = (py-(cc.shape[1]//2)) self.Axs.append(Ax) self.Ays.append(Ay) #store q value in N0s self.N0s.append(q) return Ax,Ay class ORB(Method): def __init__(self, patchSize=64, fastThreshold=36, distance_threshold=64 ): """ Setup ORB matching method. patchSize - size of patch used to find BRIEF descriptor fastThreshold - FAST feature detection threshold (2*image mean seems good) distance_threshold - reject matches with distance greater than this """ Method.__init__(self, 'ORB') cv.ocl.setUseOpenCL(False) #work around for orb bug - this needs to be called first nFeatures= 1000 #max number of features to find scaleFactor = 1.2 #pyramid scale factor nLevels = 1 #pyramid levels 1 as no scale changes edgeThreshold = patchSize #size of border to not detect in firstLevel = 0 #zero from opencv docs WTA_K = 2 #he number of points that produce each element of the #oriented BRIEF descriptor. The default value 2 means the #BRIEF where we take a random point pair and compare their #brightnesses, so we get 0/1 response (index of brightest) scoreType = cv.ORB_FAST_SCORE # ORB_HARRIS_SCORE or ORB_FAST_SCORE self.orb = cv.ORB_create( nFeatures, scaleFactor, nLevels, edgeThreshold, firstLevel, WTA_K, scoreType, patchSize, fastThreshold) #create BFMatcher object self.matcher = cv.BFMatcher(cv.NORM_HAMMING, crossCheck=True) self.distance_threshold = distance_threshold # reject matches with distance greater than this def calc_shift_and_rotation(self, img0, img1, adj_fast_threshold=True, maxMatches=None): #find kps in img0 and img1 if adj_fast_threshold: self.orb.setFastThreshold( int(img0.mean()) ) #adjust fast threshold kp0, des0 = self.orb.detectAndCompute(img0, mask=None) #find kps in img1 if adj_fast_threshold: self.orb.setFastThreshold( int(img1.max()*0.1) ) #adjust fast threshold kp1, des1 = self.orb.detectAndCompute(img1, mask=None) #print 'found {0}, {1} keypoints'.format(len(kp0),len(kp1)) # Match descriptors. matches = self.matcher.match(des0,des1) # Sort them in the order of their distance. matches = sorted(matches, key = lambda x:x.distance) #filter out those whoose distance is too great. fmatches = filter( lambda x:x.distance