Linslab's picture
Move frozen Astra policies to astra/<task>/ (BuildTent-v1 .. DualArmFoldHandKerchief-v1)
592ea41 verified
Raw History Blame Contribute Delete
1.3 kB
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