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
8369602b
Commit
8369602b
authored
Nov 21, 2020
by
czb5793
Browse files
Rewrite the inverse_sensor_model. Before that, the occupied cells couldn't be shown
parent
d6d58845
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
27 additions
and
18 deletions
+27
-18
plotters/MappingPlotter.py
plotters/MappingPlotter.py
+8
-2
plotters/SlamPlotter.py
plotters/SlamPlotter.py
+2
-1
supervisor/slam/mapping.py
supervisor/slam/mapping.py
+17
-15
No files found.
plotters/MappingPlotter.py
View file @
8369602b
from
math
import
log
class
MappingPlotter
:
def
__init__
(
self
,
slam_mapping
,
viewer
,
frame_number
):
...
...
@@ -25,9 +26,14 @@ class MappingPlotter:
for
j
in
range
(
H
):
for
i
in
range
(
W
):
val
=
map
[
j
,
i
]
# the occupancy probability
if
val
>=
0.5
:
if
abs
(
val
-
0.5
)
<
0.0001
:
# unknown area
x
,
y
=
self
.
__to_meter
(
i
,
j
)
frame
.
add_rectangle
([
x
,
y
],
self
.
pixel_size
,
self
.
pixel_size
,
color
=
(
0.5
,
0.5
,
0.5
),
alpha
=
0.5
)
frame
.
add_rectangle
([
x
,
y
],
self
.
pixel_size
,
self
.
pixel_size
,
color
=
(
0.5
,
0.5
,
0.5
),
alpha
=
0.3
)
elif
val
>
0.5
:
# occupied area
x
,
y
=
self
.
__to_meter
(
i
,
j
)
val
=
1
-
val
frame
.
add_rectangle
([
x
,
y
],
self
.
pixel_size
,
self
.
pixel_size
,
color
=
(
val
,
val
,
val
),
alpha
=
0.8
)
self
.
draw_path_planning_to_frame
(
frame
)
self
.
_draw_goal_to_frame
(
frame
)
...
...
plotters/SlamPlotter.py
View file @
8369602b
...
...
@@ -23,6 +23,7 @@ from matplotlib import pyplot as plt
import
numpy
as
np
from
supervisor.slam.EKFSlam
import
EKFSlam
from
supervisor.slam.FastSlam
import
FastSlam
class
SlamPlotter
:
...
...
@@ -52,7 +53,7 @@ class SlamPlotter:
# draw all the obstacles
for
landmark
in
self
.
slam
.
get_landmarks
():
frame
.
add_circle
((
landmark
[
0
],
landmark
[
1
]),
self
.
radius
,
"
black
"
,
alpha
=
0.
6
)
frame
.
add_circle
((
landmark
[
0
],
landmark
[
1
]),
self
.
radius
,
"
darkgreen
"
,
alpha
=
0.
8
)
if
self
.
viewer
.
draw_invisibles
and
isinstance
(
self
.
slam
,
EKFSlam
):
self
.
__draw_confidence_ellipse
(
frame
)
...
...
supervisor/slam/mapping.py
View file @
8369602b
...
...
@@ -57,8 +57,8 @@ class OccupancyGridMapping2d(Mapping):
self
.
max_range
=
supervisor_interface
.
proximity_sensor_max_range
()
self
.
prob_unknown
=
0.5
# prior
self
.
prob_occ
=
0.9
9
# probability perceptual a grid is occupied
self
.
prob_free
=
0.
0
1
# probability perceptual a grid is free
self
.
prob_occ
=
0.9
# probability perceptual a grid is occupied
self
.
prob_free
=
0.1
# probability perceptual a grid is free
self
.
reset
()
# initialize the algorithm
...
...
@@ -91,7 +91,7 @@ class OccupancyGridMapping2d(Mapping):
x0
,
y0
=
self
.
__to_gridmap_position
(
x0
,
y0
)
# from position in meters to position in pixels
x1
,
y1
=
self
.
__to_gridmap_position
(
x1
,
y1
)
# from position in meters to position in pixels
points
=
bresenham_line
(
x0
,
y0
,
x1
,
y1
)
# a list of points on the line
occ_probs
=
self
.
__
calc_prob
(
points
)
# calculate the occupancy probabilities on this line
occ_probs
=
self
.
__
inverse_sensor_model
(
points
)
# calculate the occupancy probabilities on this line
observed_pixs
+=
occ_probs
inverse_sensor
=
np
.
copy
(
self
.
L0
)
# initialize the inverse-sensor-model term by prior
...
...
@@ -150,31 +150,33 @@ class OccupancyGridMapping2d(Mapping):
else
:
self
.
path
=
list
()
def
__
calc_prob
(
self
,
points
):
def
__
inverse_sensor_model
(
self
,
points
):
"""
Calculate the probability the occupied points on a segment
Inverse sensor model.
Calculate the probability the occupied points on a segment
:param points: points on a segment in pixels
:return: A list of occupancy probabilities of the grids on the segment.
A single item is (x, y, prob) where (x, y) is the prosition of a grid, and prob is the occupancy probability.
"""
a
=
0.3
*
self
.
resolution
# the thick of the obstacles
x0
,
y0
=
points
[
0
]
# start point position
xn
,
yn
=
points
[
-
1
]
# end point position
seg_length
=
sqrt
((
xn
-
x0
)
**
2
+
(
yn
-
y0
)
**
2
)
# the length of the segment
max
_range_seg
=
self
.
max_range
*
self
.
resolution
xn
,
yn
=
points
[
-
1
]
# end point position
, the position of a obstacle
z_t
=
sqrt
((
xn
-
x0
)
**
2
+
(
yn
-
y0
)
**
2
)
# the length of the segment
, z_t
z_
max
=
self
.
max_range
*
self
.
resolution
probs
=
[]
# occupancy probabilities of the grids on the segment.
for
xi
,
yi
in
points
:
r
=
sqrt
((
xi
-
x0
)
**
2
+
(
yi
-
y0
)
**
2
)
# a distance of a cell from the robot
prob
=
self
.
prob_unknown
if
seg_length
>=
max_range_seg
:
# out of range
prob
=
self
.
prob_unknown
# assign prob with prior
if
seg_length
<
max_range_seg
:
# likely to be an object
prob
=
self
.
prob_occ
# assign prob with the occupied probability
distance
=
sqrt
((
xi
-
x0
)
**
2
+
(
yi
-
y0
)
**
2
)
if
distance
<
seg_length
:
# free space
if
r
>
min
(
z_max
,
z_t
+
a
*
0.5
):
# return a prior
prob
=
self
.
prob_unknown
if
z_t
<
z_max
and
abs
(
r
-
z_t
)
<
a
*
0.5
:
# return a occ probability
prob
=
self
.
prob_occ
if
r
<
z_t
:
prob
=
self
.
prob_free
probs
.
append
((
int
(
xi
),
int
(
yi
),
prob
))
return
probs
def
blur
(
self
,
image
):
"""
Blur the map
...
...
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