Commit 8369602b authored by czb5793's avatar czb5793
Browse files

Rewrite the inverse_sensor_model. Before that, the occupied cells couldn't be shown

parent d6d58845
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)
......
......@@ -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)
......
......@@ -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.99 # probability perceptual a grid is occupied
self.prob_free = 0.01 # 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
......
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