File size: 1,298 Bytes
8e97cab
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
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