def execute(s): tpmap = n.ndarray((ARRAY_SIZE, ARRAY_SIZE), buffer=n.zeros(ARRAY_SIZE**2)) ncentroid = (s.obj.centroid[0] + s.deltapos[0], s.obj.centroid[1] + s.deltapos[1]) tpmaps = 0. ''' # Full Gaussian redistribution. 500^4 iterations --> too slow. Optimize, C, OpenCL? for r in range(len(s.obj.pmap)): for c in range(len(s.obj.pmap[0])): ar = int(round(r - s.deltapos[0] / ARRAY_SCALE)) ac = int(round(c - s.deltapos[1] / ARRAY_SCALE)) for tr in range(ARRAY_SIZE): for tc in range(ARRAY_SIZE): tpmap[tr][tc] += gaussian(0, s.unc)(toPolar((tr - ar)*ARRAY_SCALE, (tc - ac)*ARRAY_SCALE)[0]) * s.obj.pmap[ar][ac] tpmaps += tpmap[tr][tc] print('rc {0}:{1}'.format(r, c)) ''' # Hacky version. Shifts array, then estimates other points as Gaussian from new centroid. for r in range(ARRAY_SIZE): for c in range(ARRAY_SIZE): ar = int(round(r - s.deltapos[0] / ARRAY_SCALE)) ac = int(round(c - s.deltapos[1] / ARRAY_SCALE)) try: tpmap[r][c] = s.obj.pmap[ar][ac] except: tpmap[r][c] = gaussian(0, s.obj.stddev)(toPolar( ncentroid[0] - r * ARRAY_SCALE, ncentroid[1] - c * ARRAY_SCALE)[0]) tpmaps += tpmap[r][c] tpmap /= tpmaps s.obj.pmap = tpmap s.obj.update(lambda n, e: 1.)
def __init__(s, centroid, stddev): # Probability map, centroid, stddev all in N-E coordinates from origin at lower right s.pmap = n.ndarray((ARRAY_SIZE, ARRAY_SIZE), buffer=n.ones(ARRAY_SIZE**2)) s.centroid, s.stddev = centroid, stddev s.update(lambda n, e: gaussian(0, s.stddev) (toPolar(n - s.centroid[0], e - s.centroid[1])[0]))
def execute(s): tpmap = n.ndarray((ARRAY_SIZE, ARRAY_SIZE), buffer=n.zeros(ARRAY_SIZE**2)) ncentroid = (s.obj.centroid[0] + s.deltapos[0], s.obj.centroid[1] + s.deltapos[1]) tpmaps = 0. ''' # Full Gaussian redistribution. 500^4 iterations --> too slow. Optimize, C, OpenCL? for r in range(len(s.obj.pmap)): for c in range(len(s.obj.pmap[0])): ar = int(round(r - s.deltapos[0] / ARRAY_SCALE)) ac = int(round(c - s.deltapos[1] / ARRAY_SCALE)) for tr in range(ARRAY_SIZE): for tc in range(ARRAY_SIZE): tpmap[tr][tc] += gaussian(0, s.unc)(toPolar((tr - ar)*ARRAY_SCALE, (tc - ac)*ARRAY_SCALE)[0]) * s.obj.pmap[ar][ac] tpmaps += tpmap[tr][tc] print('rc {0}:{1}'.format(r, c)) ''' # Hacky version. Shifts array, then estimates other points as Gaussian from new centroid. for r in range(ARRAY_SIZE): for c in range(ARRAY_SIZE): ar = int(round(r - s.deltapos[0] / ARRAY_SCALE)) ac = int(round(c - s.deltapos[1] / ARRAY_SCALE)) try: tpmap[r][c] = s.obj.pmap[ar][ac] except: tpmap[r][c] = gaussian(0, s.obj.stddev)(toPolar(ncentroid[0] - r*ARRAY_SCALE, ncentroid[1] - c*ARRAY_SCALE)[0]) tpmaps += tpmap[r][c] tpmap /= tpmaps s.obj.pmap = tpmap s.obj.update(lambda n, e: 1.)
def bDistance(s, n, e): return gaussian(s.dist, s.distunc)( toPolar(n - s.objfrom.centroid[0], e - s.objfrom.centroid[1])[0]) + UPDATE_FLOOR
def bHeading(s, n, e): return gaussian(s.heading, s.headingunc)(toPolar( n - s.objfrom.centroid[0], e - s.objfrom.centroid[1])[1]) + UPDATE_FLOOR
def bDistance(s, n, e): return gaussian(s.dist, s.distunc)(toPolar(n - s.objfrom.centroid[0], e - s.objfrom.centroid[1])[0]) + UPDATE_FLOOR
def bHeading(s, n, e): return gaussian(s.heading, s.headingunc)(toPolar(n - s.objfrom.centroid[0], e - s.objfrom.centroid[1])[1]) + UPDATE_FLOOR
def __init__(s, centroid, stddev): # Probability map, centroid, stddev all in N-E coordinates from origin at lower right s.pmap = n.ndarray((ARRAY_SIZE, ARRAY_SIZE), buffer=n.ones(ARRAY_SIZE**2)) s.centroid, s.stddev = centroid, stddev s.update(lambda n, e: gaussian(0, s.stddev)(toPolar(n - s.centroid[0], e - s.centroid[1])[0]))