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]))
