def main(): pf = ParticleFilter(numParticles=2000) sim = Simulator(pf) sim.start() ''' pf.particles = util.Counter() pf.numParticles = 8 pf.particles[(640, 500, 0)] = 1.0 pf.particles[(640, 500, -numpy.pi/2)] = 1.0 pf.particles[(640, 500, numpy.pi/2)] = 1.0 pf.particles[(640, 500, numpy.pi)] = 1.0 pf.particles[(640, 600, 0)] = 1.0 pf.particles[(640, 600, -numpy.pi/2)] = 1.0 pf.particles[(640, 600, numpy.pi/2)] = 1.0 pf.particles[(640, 600, numpy.pi)] = 1.0 ''' sim.refresh() time.sleep(2) # Observed lines at pixels ''' lines = [ ((0, 117), (639, 125)), ((11, 116), (639, 123)), ((0, 454), (639, 430)), ((26, 452), (638, 429)), ] for line in lines: p1 = util.pixelToDistance(line[0]) p2 = util.pixelToDistance(line[1]) dist = util.pointLineDistance((0, 0), (p1, p2)) print line print 'to', (p1,p2), 'at distance', dist pf.observeLine((dist, 0)) sim.refresh(pf.getBeliefs()) time.sleep(1) ''' # Observation is a distance and heading to an unidentified line pf.observeLine((10, 0)) pf.observeLine((150, 0)) sim.refresh() time.sleep(2) # Observation is a distance and heading to an unidentified corner pf.observeCorner((150, 0)) sim.refresh() time.sleep(2) pf.elapseTime() sim.refresh() # Main logic loop dist = 150 while True: pf.observeLine((dist, 0)) sim.refresh() pf.elapseTime((0, 30, 0)) dist -= 30
def main(): pf = ParticleFilter(numParticles=1000) sim = Simulator(pf) sim.start() ''' pf.particles = util.Counter() pf.numParticles = 8 pf.particles[(640, 500, 0)] = 1.0 pf.particles[(640, 500, -numpy.pi/2)] = 1.0 pf.particles[(640, 500, numpy.pi/2)] = 1.0 pf.particles[(640, 500, numpy.pi)] = 1.0 pf.particles[(640, 600, 0)] = 1.0 pf.particles[(640, 600, -numpy.pi/2)] = 1.0 pf.particles[(640, 600, numpy.pi/2)] = 1.0 pf.particles[(640, 600, numpy.pi)] = 1.0 ''' sim.refresh() time.sleep(2) # Observation is a distance and heading to an unidentified line pf.observeLine((50, 0)) # Observation is a distance and heading to an unidentified corner pf.observeCorner((50, 0)) sim.refresh() time.sleep(2) pf.elapseTime() sim.refresh(pf.getBeliefs()) # Main logic loop dist = 50 while True: pf.observeCorner((dist, 0)) sim.refresh(pf.getBeliefs()) pf.elapseTime((50, 0, 0.1)) dist -=50
def main(): pf = ParticleFilter(numParticles=1000) sim = Simulator(pf) sim.start() ''' pf.particles = util.Counter() pf.numParticles = 8 pf.particles[(640, 500, 0)] = 1.0 pf.particles[(640, 500, -numpy.pi/2)] = 1.0 pf.particles[(640, 500, numpy.pi/2)] = 1.0 pf.particles[(640, 500, numpy.pi)] = 1.0 pf.particles[(640, 600, 0)] = 1.0 pf.particles[(640, 600, -numpy.pi/2)] = 1.0 pf.particles[(640, 600, numpy.pi/2)] = 1.0 pf.particles[(640, 600, numpy.pi)] = 1.0 ''' sim.refresh() time.sleep(2) # Observation is a distance and heading to an unidentified line pf.observeLine((50, 0)) # Observation is a distance and heading to an unidentified corner pf.observeCorner((50, 0)) sim.refresh() time.sleep(2) pf.elapseTime() sim.refresh(pf.getBeliefs()) # Main logic loop dist = 50 while True: pf.observeCorner((dist, 0)) sim.refresh(pf.getBeliefs()) pf.elapseTime((50, 0, 0.1)) dist -= 50