104 lines
3.4 KiB
Python
104 lines
3.4 KiB
Python
import random
|
|
import numpy as np
|
|
import random_numbers as rn
|
|
|
|
class Particle(object):
|
|
"""Data structure for storing particle information (state and weight)"""
|
|
def __init__(self, x=0.0, y=0.0, theta=0.0, weight=0.0):
|
|
self.x = x
|
|
self.y = y
|
|
self.theta = np.mod(theta, 2.0*np.pi)
|
|
self.weight = weight
|
|
|
|
def getX(self):
|
|
return self.x
|
|
|
|
def getY(self):
|
|
return self.y
|
|
|
|
def getTheta(self):
|
|
return self.theta
|
|
|
|
def getWeight(self):
|
|
return self.weight
|
|
|
|
def setX(self, val):
|
|
self.x = val
|
|
|
|
def setY(self, val):
|
|
self.y = val
|
|
|
|
def setTheta(self, val):
|
|
self.theta = np.mod(val, 2.0*np.pi)
|
|
|
|
def setWeight(self, val):
|
|
self.weight = val
|
|
|
|
def copy(self):
|
|
return Particle(self.x, self.y, self.theta, self.weight)
|
|
|
|
|
|
def estimate_pose(particles_list):
|
|
"""Estimate the pose from particles by computing the average position and orientation over all particles.
|
|
This is not done using the particle weights, but just the sample distribution."""
|
|
x_values = [i.getX() for i in particles_list]
|
|
y_values = [i.getY() for i in particles_list]
|
|
cos_values = [np.cos(i.getTheta()) for i in particles_list]
|
|
sin_values = [np.sin(i.getTheta()) for i in particles_list]
|
|
|
|
x_sum = sum(x_values)
|
|
y_sum = sum(y_values)
|
|
cos_sum = sum(cos_values)
|
|
sin_sum = sum(sin_values)
|
|
|
|
flen = len(particles_list)
|
|
if flen != 0:
|
|
x = x_sum / flen
|
|
y = y_sum / flen
|
|
theta = np.arctan2(sin_sum/flen, cos_sum/flen)
|
|
else:
|
|
x = x_sum
|
|
y = y_sum
|
|
theta = 0.0
|
|
|
|
x_certainty = np.average([abs(x - i) for i in x_values])
|
|
y_certainty = np.average([abs(y - i) for i in y_values])
|
|
cos_certainty = np.average([abs(np.cos(theta) - i) for i in cos_values])
|
|
sin_certainty = np.average([abs(np.sin(theta) - i) for i in sin_values])
|
|
|
|
certainty = np.average([x_certainty, y_certainty, cos_certainty, sin_certainty])
|
|
return Particle(x, y, theta), certainty
|
|
|
|
|
|
def move_particle(particle, delta_x, delta_y, delta_theta):
|
|
"""Move the particle by (delta_x, delta_y, delta_theta)"""
|
|
particle.setX(particle.getX() + delta_x)
|
|
particle.setY(particle.getY() + delta_y)
|
|
particle.setTheta(particle.getTheta() + delta_theta)
|
|
|
|
|
|
def add_uncertainty(particles_list, sigma, sigma_theta):
|
|
"""Add some noise to each particle in the list. Sigma and sigma_theta is the noise
|
|
variances for position and angle noise."""
|
|
for particle in particles_list:
|
|
particle.x += rn.randn(0.0, sigma)
|
|
particle.y += rn.randn(0.0, sigma)
|
|
particle.theta = np.mod(particle.theta + rn.randn(0.0, sigma_theta), 2.0 * np.pi)
|
|
|
|
|
|
def add_uncertainty_von_mises(particles_list, sigma, theta_kappa):
|
|
"""Add some noise to each particle in the list. Sigma and theta_kappa is the noise
|
|
variances for position and angle noise."""
|
|
for particle in particles_list:
|
|
particle.x += rn.randn(0.0, sigma)
|
|
particle.y += rn.randn(0.0, sigma)
|
|
particle.theta = np.mod(rn.rand_von_mises(particle.theta, theta_kappa), 2.0 * np.pi) - np.pi
|
|
|
|
def resample(particle_list):
|
|
weights = [p.weight for p in particle_list]
|
|
if sum(weights) == 0:
|
|
weights = [1/len(particle_list) for _ in particle_list]
|
|
new_particles = random.choices(particle_list, weights, k=len(particle_list))
|
|
particle_list = [p.copy() for p in new_particles]
|
|
return particle_list
|