Commit 5f00b756 authored by czb5793's avatar czb5793
Browse files

Add a new evaluation class, "SlamEvaluation2.py"

parent 9169c984
......@@ -169,7 +169,7 @@ slam:
detected_angle: 30
ekf_slam:
# Determines whether the EKF SLAM algorithm shall be executed
enabled: false
enabled: true
# The mahalanobis distance threshold used in data association
distance_threshold: 1
# Configures the motion noise. The values are currently empirically chosen.
......
......@@ -401,12 +401,8 @@ class Viewer:
:param widget: The corresponding widget
"""
if self.simulator.ekfslam_evaluation is not None:
self.simulator.ekfslam_evaluation.plot()
if self.simulator.fastslam_evaluation is not None:
self.simulator.fastslam_evaluation.plot()
if self.simulator.graphbasedslam_evaluation is not None:
self.simulator.graphbasedslam_evaluation.plot()
if self.simulator.slam_evaluations is not None:
self.simulator.slam_evaluations.plot()
def on_plot_covariances(self, widget):
......
......@@ -24,7 +24,7 @@ import gi
from gi.repository import GLib
from plotters.SlamPlotter import *
from supervisor.slam.SlamEvaluation import SlamEvaluation
from supervisor.slam.SlamEvaluation2 import SlamEvaluation2
from plotters.MappingPlotter import MappingPlotter
......@@ -122,25 +122,18 @@ class Simulator:
self.ekfslam_plotter = SlamPlotter(self.world.supervisors[0].ekfslam, self.viewer, 0.04, self.cfg["robot"], n_frame)
if cfg["slam"]["mapping"]["enabled"]:
self.ekfslam_mapping_plotter = MappingPlotter(self.world.supervisors[0].ekfslam_mapping, self.viewer, n_frame)
if self.cfg["slam"]["evaluation"]["enabled"]:
self.ekfslam_evaluation = SlamEvaluation(self.world.supervisors[0].ekfslam, self.cfg["slam"]["evaluation"], self.world.robots[0])
n_frame += 1
if cfg["slam"]["fast_slam"]["enabled"]:
self.fastslam_plotter = SlamPlotter(self.world.supervisors[0].fastslam, self.viewer, 0.04, self.cfg["robot"], n_frame)
if cfg["slam"]["mapping"]["enabled"]:
self.fastslam_mapping_plotter = MappingPlotter(self.world.supervisors[0].fastslam_mapping, self.viewer, n_frame)
if self.cfg["slam"]["evaluation"]["enabled"]:
self.fastslam_evaluation = SlamEvaluation(self.world.supervisors[0].fastslam, self.cfg["slam"]["evaluation"], self.world.robots[0])
n_frame += 1
if cfg["slam"]["graph_based_slam"]["enabled"]:
self.graphbasedslam_plotter = GraphSlamPlotter(self.world.supervisors[0].graphbasedslam, self.viewer, 0.04, self.cfg["robot"], n_frame)
if cfg["slam"]["mapping"]["enabled"]:
self.graphbasedslam_mapping_plotter = MappingPlotter(self.world.supervisors[0].graphbasedslam_mapping, self.viewer, n_frame)
if self.cfg["slam"]["evaluation"]["enabled"]:
self.graphbasedslam_evaluation = SlamEvaluation(self.world.supervisors[0].graphbasedslam,
self.cfg["slam"]["evaluation"], self.world.robots[0])
n_frame += 1
# register mapping plotters to the system
......@@ -148,6 +141,13 @@ class Simulator:
self.graphbasedslam_mapping_plotter]
# register slam plotters to the system
self.reg_slam_plotters = [self.ekfslam_plotter, self.fastslam_plotter, self.graphbasedslam_plotter]
# register slam estimations to the system
self.reg_slam_evaluations = [self.ekfslam_evaluation, self.fastslam_evaluation, self.graphbasedslam_evaluation]
if self.cfg["slam"]["evaluation"]["enabled"]:
list_slam = [self.world.supervisors[0].ekfslam, self.world.supervisors[0].fastslam, self.world.supervisors[0].graphbasedslam]
self.slam_evaluations = SlamEvaluation2(list_slam,
self.cfg["slam"]["evaluation"], self.world.robots[0])
# render the initial world
self.draw_world()
......@@ -235,12 +235,7 @@ class Simulator:
def _update_slam_accuracies(self):
# Only perform the SLAM evaluation on specific simulation cycles. The period is configurable.
if self.num_cycles % self.cfg["slam"]["evaluation"]["interval"] == 0:
if self.ekfslam_evaluation is not None:
self.ekfslam_evaluation.evaluate(self.world.obstacles)
if self.fastslam_evaluation is not None:
self.fastslam_evaluation.evaluate(self.world.obstacles)
if self.graphbasedslam_evaluation is not None:
self.graphbasedslam_evaluation.evaluate(self.world.obstacles)
self.slam_evaluations.evaluate(self.world.obstacles)
def _step_sim(self):
self.num_cycles += 1
......
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 models.obstacles.FeaturePoint import FeaturePoint
class SlamEvaluation2:
def __init__(self, list_slam, evaluation_cfg, robot):
"""
Initializes an object of the SlamEvaluation2 class
:param list_slam: a list of slam algorithms that will be evaluated
:param evaluation_cfg: The configurations for the class.
Currently only used to calculate number of simulation cycles
:param robot: robot object in the real world
"""
self.list_slam = list_slam
self.cfg = evaluation_cfg
self.average_distances_lm = [list() for _ in list_slam]
self.distances_robot = [list() for _ in list_slam]
self.robot = robot
def evaluate(self, obstacles):
"""
Evaluates the average distance of the estimated obstacle positions to the closest actual obstacle in the map.
The value is saved.
:param obstacles: The list of actual obstacles of the map
"""
for j, slam in enumerate(self.list_slam):
if slam is None:
continue
slam_obstacles = slam.get_landmarks()
squared_distances = []
for i, slam_obstacle in enumerate(slam_obstacles):
slam_obstacle_id = slam_obstacle[2]
for obstacle in obstacles:
if type(obstacle) == FeaturePoint and obstacle.id == slam_obstacle_id:
sq_dist = self.__calc_squared_distance(slam_obstacle[:2], obstacle.pose.sunpack())
squared_distances.append(sq_dist)
self.average_distances_lm[j].append(sum(squared_distances) / len(squared_distances))
slam_pose = slam.get_estimated_pose()
self.distances_robot[j].append(self.__calc_squared_distance(slam_pose.sunpack(), self.robot.pose.sunpack()))
def plot(self):
"""
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()
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"
# 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')
plt.savefig('slam_landmark_evaluation.png')
ax.grid()
plt.show()
fig, ax = plt.subplots()
ax.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"
# 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')
plt.savefig('slam_robot_position_evaluation.png')
ax.grid()
plt.show()
@staticmethod
def __calc_squared_distance(x, y):
"""
Calculates squared distance between two positions.
The squared distance is sufficient for finding the minimum distance.
:param x: First position
:param y: Second position
:return: squared distance between the two positions
"""
diff = (x[0] - y[0], x[1] - y[1])
return diff[0] ** 2 + diff[1] ** 2
\ No newline at end of file
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