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