def main(DEBUG=False):
m = Map(imagePath, scale=100, sampleResolution=26)
d = Drone(m)
pf = ParticleFilter(m, d, numParticles=1000)
pf.calculateLikelihood()
pf.drawParticles()
# embed()
finish = False
manualMoveIncrement = m.scale_percent
while not finish:
update = False
dp = (0, 0)
util.drawCircle(m.image, m.positionToPixel(d.pos))
m.show()
m.clearImage()
# cv.imshow('image', m.sample(d.pos, sample_resolution=sampleWidth))
key = cv.waitKey(0)
if (key == ord('q')):
finish = True
elif (key == ord('w')):
dp = (0, -manualMoveIncrement)
d.move(dp)
update = True
elif (key == ord('s')):
dp = (0, manualMoveIncrement)
d.move(dp)
update = True
elif (key == ord('a')):
dp = (-manualMoveIncrement, 0)
d.move(dp)
update = True
elif (key == ord('d')):
dp = (manualMoveIncrement, 0)
d.move(dp)
update = True
elif (key == 13): #enter
dp = d.generateRandomMovementVector_map()
d.move(dp)
update = True
else:
print("Unrecognized key")
if (DEBUG):
distance = sum([util.distance(p.pos, d.pos)
for p in pf.particles]) / len(pf.particles)
print("true: ", d.pos)
[
print("{:01.2f} : {:01.2f}".format(p.pos[0], p.pos[1]))
for p in pf.particles
]
print("distance: ", distance)
pf.fullUpdate(dp)
def generateDataPoint1(nP, resampleRate, numIterations, headless):
ret = []
m = Map(imagePath, scale=100, sampleResolution=26)
d = Drone(m)
pf = ParticleFilter(m, d, numParticles=nP, resampleRate=resampleRate)
pf.calculateLikelihood()
for i in range(numIterations):
dp = d.generateRandomMovementVector_map()
d.move(dp)
pf.fullUpdate(dp)
# ret.append(pf.averageParticleDistance(d.pos))
ret.append(pf.numParticlesClusteredAroundGroundTruth(d.pos))
if not headless:
m.clearImage()
pf.drawParticles()
util.drawCircle(m.image, m.positionToPixel(d.pos))
m.show()
key = cv.waitKey(0)
return ret
