Commit 133d1a76 authored by czb5793's avatar czb5793
Browse files

Use other way to evaluate error, namely through the given landmark...

Use other way to evaluate error, namely through the given landmark identifiers, instead of euclidean distances.
parent 9485b8da
......@@ -123,7 +123,7 @@ class Simulator:
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.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"]:
......@@ -131,7 +131,7 @@ class Simulator:
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.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"]:
......@@ -140,7 +140,7 @@ class Simulator:
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.cfg["slam"]["evaluation"], self.world.robots[0])
n_frame += 1
# render the initial world
......
......@@ -4,18 +4,22 @@ 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 SlamEvaluation:
def __init__(self, slam, evaluation_cfg):
def __init__(self, slam, evaluation_cfg, robot):
"""
Initializes an object of the SlamEvaluation class
:param slam: The slam algorithm 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.slam = slam
self.cfg = evaluation_cfg
self.average_distances = []
self.average_distances_lm = []
self.distances_robot = []
self.robot = robot
def evaluate(self, obstacles):
"""
......@@ -24,8 +28,22 @@ class SlamEvaluation:
:param obstacles: The list of actual obstacles of the map
"""
slam_obstacles = self.slam.get_landmarks()
min_distances = [self.__find_min_distance(slam_obstacle, obstacles) for slam_obstacle in slam_obstacles]
self.average_distances.append(sum(min_distances) / len(min_distances))
# min_distances = [self.__find_min_distance(slam_obstacle, obstacles) for slam_obstacle in slam_obstacles]
# average_distance =
#squared_distances = [self.__calc_squared_distance(slam_obstacle, obstacle.pose.sunpack()) for obstacle in
# obstacles]
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.append(sum(squared_distances) / len(squared_distances))
slam_pose = self.slam.get_estimated_pose()
self.distances_robot.append(self.__calc_squared_distance(slam_pose.sunpack(), self.robot.pose.sunpack()))
def plot(self):
"""
......@@ -34,8 +52,9 @@ class SlamEvaluation:
"""
fig, ax = plt.subplots()
# Calculates number of elapsed simulation cycles
sim_cycles = len(self.average_distances) * self.cfg["interval"]
ax.plot(range(0, sim_cycles, self.cfg["interval"]), self.average_distances)
sim_cycles = len(self.average_distances_lm) * self.cfg["interval"]
ax.plot(range(0, sim_cycles, self.cfg["interval"]), self.average_distances_lm)
ax.plot(range(0, sim_cycles, self.cfg["interval"]), self.distances_robot)
ax.grid()
if isinstance(self.slam, EKFSlam):
ax.set(xlabel='Simulation cycles', ylabel='Average distance to true landmark in meters',
......
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