Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Open sidebar
Yixing Lin
sobot-rimulator
Commits
c7bf316e
Commit
c7bf316e
authored
Nov 21, 2020
by
czb5793
Browse files
Two method for data association:
1. by identifiers 2. by minimum distances
parent
8369602b
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
36 additions
and
15 deletions
+36
-15
config.yaml
config.yaml
+4
-1
supervisor/slam/SlamEvaluation.py
supervisor/slam/SlamEvaluation.py
+0
-5
supervisor/slam/SlamEvaluation2.py
supervisor/slam/SlamEvaluation2.py
+32
-9
No files found.
config.yaml
View file @
c7bf316e
...
...
@@ -169,7 +169,7 @@ slam:
detected_angle
:
30
ekf_slam
:
# Determines whether the EKF SLAM algorithm shall be executed
enabled
:
t
r
ue
enabled
:
tu
r
e
# 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
:
...
...
supervisor/slam/SlamEvaluation.py
View file @
c7bf316e
...
...
@@ -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
]
...
...
supervisor/slam/SlamEvaluation2.py
View file @
c7bf316e
...
...
@@ -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
,
obstacle
s
)
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
):
"""
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment