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