showall(landmarks, robot, 30)
return data
def showall(landmarks, robot, waiting=0):
tmp = np.zeros((world_size, world_size, 3))
tmp[:] = 255
for l in landmarks:
cv2.circle(tmp, (int(l[0]), int(l[1])), 3, (255, 0, 0))
cv2.circle(tmp, (int(robot.x), int(robot.y)), 3, (0, 255, 0))
cv2.imshow('', tmp)
cv2.waitKey(waiting)
if __name__ == '__main__':
robot = Robot(30., 30., 40)
path = [
[1., 1.],
[1., 1.],
[1., 1.],
[1., 1.],
]
for i in xrange(20):
path.append([0.0, 5.0])
data = (make_data(path, landmarks, robot))
print data
mu, om = online_slam(data, len(landmarks), 1., 1., (30., 30.))
mu.show('mu')
print robot.x, robot.y
showall(landmarks, robot, 1000)
def make_data(steps, landmarks, robot):
data = []
for st in steps:
Z, alr_seen = robot.sense()
robot.simple_move(st[0], st[1])
data.append([Z, [st[0], st[1]]])
showall(landmarks, robot, 30)
return data
def showall(landmarks, robot, waiting=0):
tmp = np.zeros((world_size, world_size, 3))
tmp[:] = 255
for l in landmarks:
cv2.circle(tmp, (int(l[0]), int(l[1])), 3, (255, 0, 0))
cv2.circle(tmp, (int(robot.x), int(robot.y)), 3, (0, 255, 0))
cv2.imshow('', tmp)
cv2.waitKey(waiting)
if __name__ == '__main__':
robot = Robot(30., 30., 40)
path = [[1., 1.], [1., 1.], [1., 1.], [1., 1.],]
for i in xrange(20):
path.append([0.0, 5.0])
data = (make_data(path, landmarks, robot))
print data
mu, om = online_slam(data, len(landmarks), 1., 1., (30., 30.))
mu.show('mu')
print robot.x, robot.y
showall(landmarks, robot, 1000)
for st in steps:
Z = agent.sense()
pos = agent.move(st)
data += [[Z, pos]]
return data
landmarks = [
[10., 20.],
[40., 8.],
[35., 120.],
]
world_size = 200
if __name__ == '__main__':
env = SimpleEnv(landmarks)
robot = Robot([30., 30.], 40, env, 0.01, 0.01)
path = [[1., 1.], [1., 1.], [1., 1.], [1., 1.],]
for i in xrange(2):
path.append([0.0, 5.0])
data = make_data(path, env, robot)
print data
mu, om = online_slam(data, len(robot.already_seen_landmark), 1., 1., (30., 30.))
mu.show('mu')
print robot.pos[0], robot.pos[1]
print robot.already_seen_landmark
#showall(landmarks, robot, 1000)
