Commit c7bf316e authored by czb5793's avatar czb5793
Browse files

Two method for data association:

1. by identifiers
2. by minimum distances
parent 8369602b
......@@ -169,7 +169,7 @@ slam:
detected_angle: 30
ekf_slam:
# Determines whether the EKF SLAM algorithm shall be executed
enabled: true
enabled: ture
# The mahalanobis distance threshold used in data association
distance_threshold: 1
# Configures the motion noise. The values are currently empirically chosen.
......@@ -225,6 +225,9 @@ slam:
# Determines the interval of when the accuracy of the generated maps is calculated
# A low interval (for example 1) causes performance problems
interval: 18
# Determine the method of data association,
# if true, data wil be associated by landmark identifiers, otherwise by minimum distance
associate_by_id: true
# Configures the 2D grid map of the occupancy grid mapping algorithm
mapping:
......
......@@ -28,11 +28,6 @@ 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]
# 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]
......
......@@ -32,18 +32,41 @@ class SlamEvaluation2:
if slam is None:
continue
slam_obstacles = slam.get_landmarks()
squared_distances = []
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)
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)
self.average_distances_lm[j].append(sum(squared_distances) / len(squared_distances))
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()))
slam_pose = slam.get_estimated_pose()
self.distances_robot[j].append(self.__calc_squared_distance(slam_pose.sunpack(), self.robot.pose.sunpack()))
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))
def plot(self):
"""
......
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