SlamEvaluation2.py 5.23 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34
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()
czb5793's avatar
czb5793 committed
35
      distances = []
36
      for i, slam_obstacle in enumerate(slam_obstacles):
czb5793's avatar
czb5793 committed
37 38 39 40 41
        if self.cfg["associate_by_id"] == True:
          dist = self.__find_distance_by_landmark_id(slam_obstacle, obstacles)
        else:
          dist = self.__find_min_distance(slam_obstacle, obstacles)
        distances.append(dist)
42

czb5793's avatar
czb5793 committed
43 44 45 46
      if (len(distances)) > 0:
        self.average_distances_lm[j].append(sum(distances) / len(distances))
        slam_pose = slam.get_estimated_pose()
        self.distances_robot[j].append(self.__calc_squared_distance(slam_pose.sunpack(), self.robot.pose.sunpack()))
47

czb5793's avatar
czb5793 committed
48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
  def __find_distance_by_landmark_id(self, slam_obstacle, obstacles):
    """
    Finds the distance of the estimated obstacle to the actual obstacle regarding its identifier
    :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 the actual obstacle
    """
    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())
        return sqrt(sq_dist)

  def __find_min_distance(self, slam_obstacle, obstacles):
      """
      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
      """
      squared_distances = [self.__calc_squared_distance(slam_obstacle, obstacle.pose.sunpack()) for obstacle in obstacles]
      return sqrt(min(squared_distances))
70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134

  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