✨

selflocalization/particle.py

@@ -41,16 +41,15 @@ class Particle(object):
 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_sum = 0.0
-    y_sum = 0.0
-    cos_sum = 0.0
-    sin_sum = 0.0
+    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]
 
-    for particle in particles_list:
-        x_sum += particle.getX()
-        y_sum += particle.getY()
-        cos_sum += np.cos(particle.getTheta())
-        sin_sum += np.sin(particle.getTheta())
+    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:
@@ -62,7 +61,13 @@ def estimate_pose(particles_list):
         y = y_sum
         theta = 0.0
 
-    return Particle(x, y, theta)
+    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):

selflocalization/selflocalize.py

@@ -215,17 +215,12 @@ try:
     else:
         cam = camera.Camera(0, 'macbookpro', useCaptureThread = True)
 
-    arlo.go_diff(40, 40, 0, 1)
 
-    for i in range(100):
-        # Move the robot according to user input (only for testing)
-        print(i)
-        action = cv2.waitKey(10)
-        if action == ord('q'): # Quit
-            break
+    while True:
 
         # Fetch next frame
-        colour = cam.get_next_frame()
+        # colour = cam.get_next_frame()
+        colour = arlo.take_photo()
 
         # Detect objects
         objectIDs, dists, angles = cam.detect_aruco_objects(colour)
@@ -254,7 +249,8 @@ try:
                 p.setWeight(1.0/num_particles)
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
-        est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+        est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
+        print(certainty)
 
         if showGUI:
             # Draw map
@@ -266,8 +262,7 @@ try:
             # Show world
             cv2.imshow(WIN_World, world)
 
-    arlo.stop()
-    drive_towards_middle(est_pose, arlo)
+    # drive_towards_middle(est_pose, arlo)
 
 
 finally: