NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-10 14:00:45 +02:00
parent f77b2d32bd
commit 3a3903176a
2 changed files with 24 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
selflocalization/particle.py
View File

@ -41,16 +41,15 @@ class Particle(object):
def estimate_pose(particles_list): def estimate_pose(particles_list):
"""Estimate the pose from particles by computing the average position and orientation over all particles. """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.""" This is not done using the particle weights, but just the sample distribution."""
x_sum = 0.0 x_values = [i.getX() for i in particles_list]
y_sum = 0.0 y_values = [i.getY() for i in particles_list]
cos_sum = 0.0 cos_values = [np.cos(i.getTheta()) for i in particles_list]
sin_sum = 0.0 sin_values = [np.sin(i.getTheta()) for i in particles_list]
for particle in particles_list: x_sum = sum(x_values)
x_sum += particle.getX() y_sum = sum(y_values)
y_sum += particle.getY() cos_sum = sum(cos_values)
cos_sum += np.cos(particle.getTheta()) sin_sum = sum(sin_values)
sin_sum += np.sin(particle.getTheta())
flen = len(particles_list) flen = len(particles_list)
if flen != 0: if flen != 0:
@ -62,7 +61,13 @@ def estimate_pose(particles_list):
y = y_sum y = y_sum
theta = 0.0 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): def move_particle(particle, delta_x, delta_y, delta_theta):

17
selflocalization/selflocalize.py
View File

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