def main():
# initialize OpenCV windows
cv.NamedWindow('frame', cv.CV_WINDOW_AUTOSIZE)
cv.MoveWindow('frame', 10, 10)
cv.NamedWindow('edges', cv.CV_WINDOW_AUTOSIZE)
cv.MoveWindow('edges', 200, 10)
# initialize Simulator window
pf = ParticleFilter(numParticles=1000)
sim = Simulator(pf)
# sim.start()
sim.refresh()
time.sleep(1)
frame = cv.LoadImage(sys.argv[1])
if frame is None:
print 'Error loading image %s' % sys.argv[1]
return
find_corners(frame, pf)
sim.refresh()
# Pause for key press
while True:
k = cv.WaitKey(33)
if k == 'q':
break
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
number_of_particles = 75
num_landmarks = 60
world_size, num_landmarks, landmarks, position_at_i, measurements_at_i, u_at_i = \
generate_simulation(num_landmarks, x_min, x_max, orig_state=[5, 50, 0], threshold=None,
number_of_steps=number_of_steps, f=f, Q=Q, R=R, step_size=3,
additional_args=(w, delta_t), dim_state=3)
# set up the Extended Kalman filter to use
ExKF = ExtendedKalmanFilter(position_at_i[0], np.eye(3), dim_state=3, dim_z=2, Q=Q*Q_inflation, R=R)
# set up the Ensemble Kalman filter to use
EnsKF = EnsembleKalmanFilter(position_at_i[0], number_of_ensemble_members, dim_state=3, dim_z=2, Q=Q*Q_inflation, R=R, init_cov=init_cov)
# set up the particle filter to use
PF = ParticleFilter(number_of_particles, position_at_i[0], Q*Q_inflation, R, init_args=('gaussian', init_cov))
ex_prediction = [ExKF.state]
ens_prediction = [EnsKF.state]
pf_prediction = [PF.get_prediction()]
plot_result, plot_prediction, plot_update = True, False, False
do_ex, do_ens, do_pf = True, True, True
folder = "../output/localization/"
for i in range(1, number_of_steps+1):
print("--> Doing step %d" % i)
u = u_at_i[i]
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
