def draw_world(est_pose, particles, world):
"""Visualization.
This functions draws robots position in the world."""
offset = 100;
world[:] = CWHITE # Clear background to white
# Find largest weight
max_weight = 0
for particle in particles:
max_weight = max(max_weight, particle.getWeight())
# Draw particles
for particle in particles:
x = int(particle.getX()) + offset
y = int(particle.getY()) + offset
colour = jet(particle.getWeight() / max_weight)
cv2.circle(world, (x,y), 2, colour, 2)
# Draw landmarks
lm0 = (landmarks[0][0]+offset, landmarks[0][1]+offset)
lm1 = (landmarks[1][0]+offset, landmarks[1][1]+offset)
cv2.circle(world, lm0, 5, CRED, 2)
cv2.circle(world, lm1, 5, CGREEN, 2)
# Draw estimated robot pose
a = (int(est_pose.getX())+offset, int(est_pose.getY())+offset)
b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offset,
int(est_pose.getY() + 15.0*np.sin(est_pose.getTheta()))+offset)
cv2.circle(world, a, 5, CMAGENTA, 2)
cv2.line(world, a, b, CMAGENTA, 2)
def draw_world(est_pose, particles, world):
"""Visualization.
This functions draws robots position in the world."""
offset = 100;
world[:] = CWHITE # Clear background to white
# Find largest weight
max_weight = 0
for particle in particles:
max_weight = max(max_weight, particle.getWeight())
# Draw particles
for particle in particles:
x = int(particle.getX()) + offset
y = int(particle.getY()) + offset
colour = jet(particle.getWeight() / max_weight)
cv2.circle(world, (x,y), 2, colour, 2)
b = (int(particle.getX() + 15.0*np.cos(particle.getTheta()))+offset,
int(particle.getY() - 15.0*np.sin(particle.getTheta()))+offset)
cv2.line(world, (x,y), b, colour, 2)
# Draw landmarks
lm0 = (landmarks[0][0]+offset, landmarks[0][1]+offset)
lm1 = (landmarks[1][0]+offset, landmarks[1][1]+offset)
cv2.circle(world, lm0, 5, CRED, 2)
cv2.circle(world, lm1, 5, CGREEN, 2)
# Draw estimated robot pose
a = (int(est_pose.getX())+offset, int(est_pose.getY())+offset)
b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offset,
int(est_pose.getY() - 15.0*np.sin(est_pose.getTheta()))+offset)
cv2.circle(world, a, 5, CMAGENTA, 2)
cv2.line(world, a, b, CMAGENTA, 2)
def draw_world(est_pose, particles, world):
"""Visualization.
This functions draws robots position in the world coordinate system."""
# Fix the origin of the coordinate system
offsetX = 100
offsetY = 250
# Constant needed for transforming from world coordinates to screen coordinates (flip the y-axis)
ymax = world.shape[0]
world[:] = CWHITE # Clear background to white
# Find largest weight
max_weight = 0
for particle in particles:
max_weight = max(max_weight, particle.getWeight())
# Draw particles
for particle in particles:
x = int(particle.getX() + offsetX)
y = ymax - (int(particle.getY() + offsetY))
colour = jet(particle.getWeight() / max_weight)
cv2.circle(world, (x, y), 2, colour, 2)
b = (int(particle.getX() + 15.0 * np.cos(particle.getTheta())) +
offsetX, ymax -
(int(particle.getY() + 15.0 * np.sin(particle.getTheta())) +
offsetY))
cv2.line(world, (x, y), b, colour, 2)
# Draw landmarks
lm1 = (int(landmarks[0][0]), int(ymax - (landmarks[0][1])))
lm2 = (int(landmarks[1][0]), int(ymax - (landmarks[1][1])))
lm3 = (int(landmarks[2][0]), int(ymax - (landmarks[2][1])))
lm4 = (int(landmarks[3][0]), int(ymax - (landmarks[3][1])))
cv2.circle(world, lm1, 5, CRED, 2)
cv2.circle(world, lm2, 5, CGREEN, 2)
cv2.circle(world, lm3, 5, CGREEN, 2)
cv2.circle(world, lm4, 5, CRED, 2)
# Draw estimated robot pose
a = (int(est_pose.getX()) + offsetX,
ymax - (int(est_pose.getY()) + offsetY))
b = (int(est_pose.getX() + 15.0 * np.cos(est_pose.getTheta())) + offsetX,
ymax -
(int(est_pose.getY() + 15.0 * np.sin(est_pose.getTheta())) + offsetY))
cv2.circle(world, a, 5, CMAGENTA, 2)
cv2.line(world, a, b, CMAGENTA, 2)
def resample(self):
weights = map(lambda p: p.getWeight(), self.particles)
self.particles = pf.ParticleFilter(self.particles, weights)
