SlamEvaluation.py 4.09 KB
Newer Older
Michael Dobler's avatar
Michael Dobler committed
1 2 3
from math import sqrt
from matplotlib import pyplot as plt

4
from supervisor.slam.EKFSlam import EKFSlam
5 6
from supervisor.slam.FastSlam import FastSlam
from supervisor.slam.GraphBasedSLAM import GraphBasedSLAM
7
from models.obstacles.FeaturePoint import FeaturePoint
Michael Dobler's avatar
Michael Dobler committed
8 9

class SlamEvaluation:
10
    def __init__(self, slam, evaluation_cfg, robot):
11 12 13 14 15
        """
        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
16
        :param robot: robot object in the real world
17
        """
Michael Dobler's avatar
Michael Dobler committed
18
        self.slam = slam
19
        self.cfg = evaluation_cfg
20 21 22
        self.average_distances_lm = []
        self.distances_robot = []
        self.robot = robot
Michael Dobler's avatar
Michael Dobler committed
23

24 25 26 27 28 29
    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
        """
Michael Dobler's avatar
Michael Dobler committed
30
        slam_obstacles = self.slam.get_landmarks()
31 32 33 34 35 36 37 38 39 40 41
        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()))
Michael Dobler's avatar
Michael Dobler committed
42 43

    def plot(self):
44 45 46 47
        """
        Produces a plot of how the average distance changed over the course of the simulation.
        Saves the plot in a png file.
        """
Michael Dobler's avatar
Michael Dobler committed
48
        fig, ax = plt.subplots()
49
        # Calculates number of elapsed simulation cycles
50 51 52
        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)
Michael Dobler's avatar
Michael Dobler committed
53
        ax.grid()
54
        if isinstance(self.slam, EKFSlam):
Michael Dobler's avatar
Michael Dobler committed
55
            ax.set(xlabel='Simulation cycles', ylabel='Average distance to true landmark in meters',
Michael Dobler's avatar
Michael Dobler committed
56
                   title='Evaluation of EKF SLAM')
Michael Dobler's avatar
Michael Dobler committed
57
            plt.savefig('ekf_slam_evaluation.png')
58
        elif isinstance(self.slam, FastSlam):
Michael Dobler's avatar
Michael Dobler committed
59
            ax.set(xlabel='Simulation cycles', ylabel='Average distance to true landmark in meters',
Michael Dobler's avatar
Michael Dobler committed
60
                   title='Evaluation of FastSLAM')
Michael Dobler's avatar
Michael Dobler committed
61
            plt.savefig('fast_slam_evaluation.png')
62 63 64 65 66
        elif isinstance(self.slam, GraphBasedSLAM):
            ax.set(xlabel='Simulation cycles', ylabel='Average distance to true landmark in meters',
                   title='Evaluation of Graph-based Slam')
            plt.savefig('graph_based_slam_evaluation.png')

Michael Dobler's avatar
Michael Dobler committed
67 68 69 70
        ax.grid()

        plt.show()

Michael Dobler's avatar
Michael Dobler committed
71
    def __find_min_distance(self, slam_obstacle, obstacles):
72 73 74 75 76 77
        """
        Finds the distance of the estimated obstacle to the the closest actual obstacle
        :param slam_obstacle: An estimated obstacle position of a SLAM algorithm
        :param obstacles: The list of actual obstacles in the map
        :return: Distance of estimated obstacle to closest actual obstacle
        """
Michael Dobler's avatar
Michael Dobler committed
78
        squared_distances = [self.__calc_squared_distance(slam_obstacle, obstacle.pose.sunpack()) for obstacle in obstacles]
79
        return sqrt(min(squared_distances))
Michael Dobler's avatar
Michael Dobler committed
80

Michael Dobler's avatar
Michael Dobler committed
81 82
    @staticmethod
    def __calc_squared_distance(x, y):
83 84
        """
        Calculates squared distance between two positions.
Michael Dobler's avatar
Michael Dobler committed
85
        The squared distance is sufficient for finding the minimum distance.
86 87 88 89
        :param x: First position
        :param y: Second position
        :return: squared distance between the two positions
        """
Michael Dobler's avatar
Michael Dobler committed
90
        diff = (x[0] - y[0], x[1] - y[1])
91
        return diff[0] ** 2 + diff[1] ** 2