✨

selflocalization/particle.py

@@ -1,6 +1,6 @@
+import random
 import numpy as np
-import selflocalization.random_numbers as rn
-
+import random_numbers as rn
 
 class Particle(object):
     """Data structure for storing particle information (state and weight)"""
@@ -34,6 +34,9 @@ class Particle(object):
     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. 
@@ -64,7 +67,9 @@ def estimate_pose(particles_list):
      
 def move_particle(particle, delta_x, delta_y, delta_theta):
     """Move the particle by (delta_x, delta_y, delta_theta)"""
-    print("particle.py: move_particle not implemented. You should do this.") 
+    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):
@@ -83,3 +88,11 @@ def add_uncertainty_von_mises(particles_list, sigma, theta_kappa):
         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