import cv2,numpy as np def marker_features(rgb): f=[] for c in ['front_camera','left_camera']: im=np.asarray(rgb[c]).squeeze() for mask,count in [((im[:,:,0]>170)&(im[:,:,1]<100)&(im[:,:,2]<100),4),((im[:,:,2]>170)&(im[:,:,0]<100)&(im[:,:,1]<130),2)]: n,l,s,p=cv2.connectedComponentsWithStats(mask.astype(np.uint8)) z=sorted([(ss[4],pp) for ss,pp in zip(s[1:],p[1:]) if ss[4]>=3],reverse=True,key=lambda x:x[0])[:count] p=np.array([x[1]/[im.shape[1],im.shape[0]] for x in z]).reshape(-1,2) if len(p):p=p[np.argsort(p[:,0])] a=np.full((count,2),-1.,np.float32);a[:len(p)]=p;f.extend(a.flatten()) return np.array(f,np.float32) def marker_distance(bank,current): # Color and camera are preserved; point order within a color is irrelevant. ds=np.zeros(len(bank),np.float32) for start,n in [(0,4),(8,2),(12,4),(20,2)]: b=bank[:,start:start+2*n].reshape(-1,n,2);c=current[start:start+2*n].reshape(n,2);validc=c[:,0]>=0;validb=b[:,:,0]>=0 if not validc.any():continue dd=np.sum((b[:,:,None,:]-c[None,None,:,:])**2,axis=-1);dd=np.where(validb[:,:,None]&validc[None,None,:],dd,1.) left=np.where(validb,dd.min(2),0).sum(1)/np.maximum(validb.sum(1),1) right=np.where(validc[None,:],dd.min(1),0).sum(1)/validc.sum() ds+=np.minimum((left+right)/2,.02)/(.03**2) return ds/4