From 3a3903176acd0f685977e57baf53494a59b2e09a Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 10 Oct 2022 14:00:45 +0200
Subject: [PATCH] :sparkles:

---
 selflocalization/particle.py     | 31 ++++++++++++++++++-------------
 selflocalization/selflocalize.py | 17 ++++++-----------
 2 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/selflocalization/particle.py b/selflocalization/particle.py
index 5e7430c..7534b92 100644
--- a/selflocalization/particle.py
+++ b/selflocalization/particle.py
@@ -41,17 +41,16 @@ 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
-     
-    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_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
@@ -61,8 +60,14 @@ def estimate_pose(particles_list):
         x = x_sum
         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):
diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index 8bc13fe..d81851f 100644
--- a/selflocalization/selflocalize.py
+++ b/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: