Commit 3df80973 authored by czb5793's avatar czb5793
Browse files

1. Add a callback function for evaluating time used for updates.

2. Record slam data and save as csv files
parent 2f336ea7
......@@ -3,7 +3,7 @@ EKF SLAM
Based on implementation of Atsushi Sakai (https://github.com/AtsushiSakai/PythonRobotics),
which I have also made contributions to, see pull requests #255, #258 and #305
"""
import time
import numpy as np
from math import *
from models.Pose import Pose
......@@ -14,12 +14,13 @@ from itertools import cycle
class EKFSlam(Slam):
def __init__(self, supervisor_interface, slam_cfg, step_time):
def __init__(self, supervisor_interface, slam_cfg, step_time, callback = None):
"""
Initializes an object of the EKFSlam class
:param supervisor_interface: The interface to interact with the robot supervisor
:param slam_cfg: The configuration for the SLAM algorithm
:param step_time: The discrete time that a single simulation cycle increments
:param callback: callback function
"""
# Bind the supervisor interface
self.supervisor = supervisor_interface
......@@ -41,6 +42,8 @@ class EKFSlam(Slam):
# The list of landmark IDs.
self.landmark_id = list()
self.callback = callback
def get_estimated_pose(self):
"""
Returns the estimated robot pose by retrieving the first three elements of the combined state vector
......@@ -53,7 +56,7 @@ class EKFSlam(Slam):
Returns the estimated landmark positions
:return: List of estimated landmark positions
"""
return [(x, y, id) for (x, y, id) in zip(self.mu[self.robot_state_size::2], self.mu[self.robot_state_size + 1::2], cycle(self.landmark_id))]
return [(x[0], y[0], id) for (x, y, id) in zip(self.mu[self.robot_state_size::2], self.mu[self.robot_state_size + 1::2], cycle(self.landmark_id))]
def get_covariances(self):
"""
......@@ -68,9 +71,15 @@ class EKFSlam(Slam):
:param u: Motion command
:param z: List of sensor measurements. A single measurement is a tuple of measured distance and measured angle.
"""
start_time = time.time()
self.prediction_step(u)
self.correction_step(z)
if self.callback is not None:
self.callback(str(self), time.time() - start_time) # time used for updating
def prediction_step(self, u):
"""
Predicts the robots location and location uncertainty after the execution of a motion command.
......@@ -291,3 +300,5 @@ class EKFSlam(Slam):
lm = mu[R + L * i: R + L * (i + 1), :]
return lm
def __str__(self):
return "EKF SLAM"
......@@ -8,6 +8,7 @@ Most significant changes made:
- Change resampling algorithm
"""
import time
from math import cos, sin, sqrt, atan2, exp, pi
import numpy as np
......@@ -44,12 +45,13 @@ class Particle:
class FastSlam(Slam):
def __init__(self, supervisor_interface, slam_cfg, step_time):
def __init__(self, supervisor_interface, slam_cfg, step_time, callback = None):
"""
Creates a FastSlam object
:param supervisor_interface: The interface to interact with the robot supervisor
:param slam_cfg: The configuration for the SLAM algorithm
:param step_time: The discrete time that a single simulation cycle increments
:param callback: callback function
"""
# Bind the supervisor interface
self.supervisor = supervisor_interface
......@@ -66,6 +68,7 @@ class FastSlam(Slam):
self.landmark_correspondence_given = slam_cfg["feature_detector"]
# Create initial list of particles
self.particles = [Particle(self.landmark_state_size) for _ in range(self.n_particles)]
self.callback = callback
def get_estimated_pose(self):
"""
......@@ -91,10 +94,16 @@ class FastSlam(Slam):
:param z: Sensor measurements
:return: Updated list of particles
"""
start_time = time.time()
# prediction step
self.particles = self.predict_particles(self.particles, u)
# correction step
self.correction_step(z)
if self.callback is not None:
self.callback(str(self), time.time() - start_time) # time used for updating
return self.particles
def correction_step(self, z):
......@@ -393,3 +402,5 @@ class FastSlam(Slam):
lm[0, 1] = particle.y + z[0] * sin(z[1] + particle.theta)
return lm
def __str__(self):
return "FastSLAM"
\ No newline at end of file
from math import sqrt
from matplotlib import pyplot as plt
from supervisor.slam.EKFSlam import EKFSlam
from supervisor.slam.FastSlam import FastSlam
from supervisor.slam.GraphBasedSLAM import GraphBasedSLAM
from matplotlib.ticker import MaxNLocator
from models.obstacles.FeaturePoint import FeaturePoint
import pickle
import pandas as pd
class SlamEvaluation2:
def __init__(self, list_slam, evaluation_cfg, robot):
......@@ -21,6 +19,16 @@ class SlamEvaluation2:
self.average_distances_lm = [list() for _ in list_slam]
self.distances_robot = [list() for _ in list_slam]
self.robot = robot
self.reset_record()
self.sim_circle = 0
def reset_record(self):
self.head1 = ["sim_circle", "landmark_id", "estimated_landmark_position", "estimated_robot_pose",
"actual_landmark_position", "actual_robot_pose", "slam_name"]
self.data = list()
self.head2 = ["sim_circle", "name", "time_per_update"]
self.update_time_record = list()
def evaluate(self, obstacles):
"""
......@@ -45,6 +53,44 @@ class SlamEvaluation2:
slam_pose = slam.get_estimated_pose()
self.distances_robot[j].append(self.__calc_squared_distance(slam_pose.sunpack(), self.robot.pose.sunpack()))
def record(self, sim_circle, obstacles):
"""
Record data in a list
:param sim_circle: simulation circle
"""
self.sim_circle = sim_circle
for j, slam in enumerate(self.list_slam):
if slam is None:
continue
slam_landmarks = slam.get_landmarks()
estimated_robot_position = slam.get_estimated_pose().sunpack()
actual_robot_position = self.robot.pose.sunpack()
slam_name = str(slam)
for i, slam_landmark in enumerate(slam_landmarks):
landmark_id = slam_landmark[2]
estimated_landmark_position = slam_landmark[:2]
actual_landmark_position = None
for obstacle in obstacles:
if type(obstacle) == FeaturePoint and obstacle.id == landmark_id:
actual_landmark_position = obstacle.pose.sunpack()
break
self.data.append([int(sim_circle), int(landmark_id), estimated_landmark_position,
estimated_robot_position, actual_landmark_position, actual_robot_position, slam_name])
if sim_circle % 1000 == 0:
print ("sim_circle", sim_circle)
def time_per_step(self, name, time):
"""
A callback function for recording time used per update step.
:param name: Name of slam or mapping algorithm
:param time: time cost by updating
"""
if self.sim_circle > 0:
self.update_time_record.append((self.sim_circle, name, time))
def __find_distance_by_landmark_id(self, slam_obstacle, obstacles):
"""
Finds the distance of the estimated obstacle to the actual obstacle regarding its identifiers
......@@ -73,53 +119,56 @@ class SlamEvaluation2:
Produces a plot of how the average distance changed over the course of the simulation.
Saves the plot in a png file.
"""
fig, ax = plt.subplots()
ax.grid()
self.save_infomation("./scripts/sobot_information")
fig, ax = plt.subplots(2, figsize=(9,8))
ax[0].grid()
line_styles = ['k-', 'k--', 'k:', 'k-.']
for i, slam in enumerate(self.list_slam):
if slam is None:
continue
if isinstance(slam, EKFSlam):
name = "EKF SLAM"
elif isinstance(slam, FastSlam):
name = "FastSLAM"
elif isinstance(slam, GraphBasedSLAM):
name = "Graph-based SLAM"
else:
name = "SLAM"
name = str(slam)
# Calculates number of elapsed simulation cycles
sim_cycles = len(self.average_distances_lm[i]) * self.cfg["interval"]
ax.plot(range(0, sim_cycles, self.cfg["interval"]), self.average_distances_lm[i], line_styles[i%len(self.list_slam)], label = name)
ax.legend()
ax.set(xlabel='Simulation cycles', ylabel='Average distance to true landmark in meters', title='Evaluation of SLAM')
ax[0].plot(range(0, sim_cycles, self.cfg["interval"]),
self.average_distances_lm[i],
line_styles[i%len(self.list_slam)], label = name)
ax[0].xaxis.set_major_locator(MaxNLocator(integer=True))
ax[0].legend()
ax[0].set(xlabel='Simulation cycles', ylabel='Average distance to true landmark in meters', title='Evaluation of SLAM')
plt.savefig('slam_landmark_evaluation.png')
ax.grid()
ax[0].grid()
plt.show()
fig, ax = plt.subplots()
ax.grid()
#fig, ax = plt.subplots()
ax[1].grid()
for i, slam in enumerate(self.list_slam):
if slam is None:
continue
if isinstance(slam, EKFSlam):
name = "EKF SLAM"
elif isinstance(slam, FastSlam):
name = "FastSLAM"
elif isinstance(slam, GraphBasedSLAM):
name = "Graph-based SLAM"
else:
name = "SLAM"
name = str(slam)
# Calculates number of elapsed simulation cycles
sim_cycles = len(self.average_distances_lm[i]) * self.cfg["interval"]
ax.plot(range(0, sim_cycles, self.cfg["interval"]), self.distances_robot[i], line_styles[i%len(self.list_slam)], label = name)
ax.legend()
ax.set(xlabel='Simulation cycles', ylabel='Distance to true robot position in meters', title='Evaluation of SLAM')
ax[1].plot(range(0, sim_cycles, self.cfg["interval"]), self.distances_robot[i], line_styles[i%len(self.list_slam)], label = name)
ax[1].legend()
ax[1].set(xlabel='Simulation cycles', ylabel='Distance to true robot position in meters', title='Evaluation of SLAM')
plt.savefig('slam_robot_position_evaluation.png')
ax.grid()
ax[1].grid()
plt.tight_layout()
plt.show()
def save_infomation(self, filename):
"""
Save the information
:param filename: The filename under which the information shall be stored
"""
df = pd.DataFrame(columns=self.head1, data=self.data)
df.to_csv(filename + "1.csv")
df = pd.DataFrame(columns=self.head2, data=self.update_time_record)
df.to_csv(filename + "2.csv")
@staticmethod
def __calc_squared_distance(x, y):
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment