mapping.py 9.65 KB
Newer Older
1 2
"""
This file includes
czb5793's avatar
czb5793 committed
3
- occupancy grid mapping algorithm
4 5 6 7
- path planning algorithm

"""

Yixing Lin's avatar
Yixing Lin committed
8
import numpy as np
9 10
import math
from scipy.ndimage import gaussian_filter
11 12
from math import cos, sin, sqrt
from utils.geometrics_util import bresenham_line
Yixing Lin's avatar
Yixing Lin committed
13

Yixing Lin's avatar
Yixing Lin committed
14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38
class Mapping:
    """
    An abstract class for a mapping algorithm
    """
    def update(self, z):
        """
        Update the map and the path using mapping and path planning algorithms
        :param z: Measurements data from sensors
        """
        raise NotImplementedError()

    def get_map(self):
        """
        Returns the estimated occupancy map
        """
        raise NotImplementedError()

    def get_path(self):
        """
        Returns the estimated path of planning
        """
        raise NotImplementedError()



czb5793's avatar
czb5793 committed
39
class OccupancyGridMapping2d(Mapping):
40
    def __init__ (self, slam, slam_cfg, supervisor_interface, path_planner = None):
Yixing Lin's avatar
Yixing Lin committed
41
        """
czb5793's avatar
czb5793 committed
42
        Initialize the OccupancyGridMapping2d object
Yixing Lin's avatar
Yixing Lin committed
43 44
        :param slam: The underlying slam algorithm object.
        :param slam_cfg: The slam configuration.
45
        :param path_planner: An object of PathPlanner
Yixing Lin's avatar
Yixing Lin committed
46
        """
47
        self.supervisor = supervisor_interface
Yixing Lin's avatar
Yixing Lin committed
48
        self.slam = slam
49
        self.path_planner = path_planner
Yixing Lin's avatar
Yixing Lin committed
50 51 52 53 54 55
        self.width = slam_cfg['mapping']['gridmap']['width'] # width of the map in meters
        self.height = slam_cfg['mapping']['gridmap']['height'] # height of the map in meters
        self.resolution = slam_cfg['mapping']['gridmap']['resolution'] # resolution of the map, i.e. number of pixels per meter
        self.W = int(self.width * self.resolution) # width of the map in pixels
        self.H = int(self.height * self.resolution) # height of the map in pixels
        self.offset = (slam_cfg['mapping']['gridmap']['offset']['x'],  # offset in meters in horizontal direction
56
                        slam_cfg['mapping']['gridmap']['offset']['y']) # offset in meters in vertical directionss
57
        self.max_range = supervisor_interface.proximity_sensor_max_range()
Yixing Lin's avatar
Yixing Lin committed
58

59
        self.prob_unknown = 0.5 # prior
60 61
        self.prob_occ = 0.9 # probability perceptual a grid is occupied
        self.prob_free = 0.1 # probability perceptual a grid is free
62 63 64 65 66 67 68 69 70 71

        self.reset() # initialize the algorithm



    def reset(self):
        """
        reset the map
        """
        self.update_enabled = False
72 73 74 75 76 77 78
        self.map = np.full((self.H, self.W), self.prob_unknown, dtype=np.float32)
        self.L = self.__prob2log(np.full_like(self.map, self.prob_unknown, dtype=np.float32))  # log recursive term of the map
        self.L0 = self.__prob2log(np.full_like(self.map, self.prob_unknown, dtype=np.float32))  # log prior term of the map

        self.path = list() # a path calculated by path planner
        self.update_counter = 0

79 80 81
    def update(self, z):
        """
        Update the occupancy gridmap recursively
82
        Update the path planning
83 84
        :param z: Measurement, represented as tuple of measured distance and measured angle
        """
85 86 87
        if not self.update_enabled:
            return

88
        observed_pixs = [] # a list of grid positions and its occupancy probabilities
89 90 91 92 93
        lines = self.__calc_lines(z)
        for x0, y0, x1, y1 in lines:
            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
94
            occ_probs = self.__inverse_sensor_model(points)  # calculate the occupancy probabilities on this line
95
            observed_pixs += occ_probs
96

97
        inverse_sensor = np.copy(self.L0) # initialize the inverse-sensor-model term by prior
Yixing Lin's avatar
Yixing Lin committed
98

99 100
        for xi, yi, prob_occ in observed_pixs:
            if xi < self.W and xi >= 0 and yi < self.H and yi >= 0:
101 102
                inverse_sensor[yi, xi] = math.log((prob_occ/(1-prob_occ)))
        self.L = self.L + inverse_sensor - self.L0  # update the recursive term
Yixing Lin's avatar
Yixing Lin committed
103
        self.L = np.clip(self.L, -5, 5)
104 105 106
        self.update_counter += 1
        self.__update_path_planning()      # update path planning

Yixing Lin's avatar
Yixing Lin committed
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
    def get_path(self):
        """
        Get the path from the path planner.
        :return: A list of points on the path. A single item is (x, y) in meters.
        """
        if len(self.path) > 1:
            world_path = [self.__to_world_position(xi, yi) for xi, yi in self.path]
        else:
            world_path = list()
        return world_path

    def get_map(self):
        """
        :return: a 2D numpy.array of the occupied gridmap.
                    A single value represents the probability of the occupancy
        """
        self.map = self.__log2prob(self.L)
        return self.map

    def map_shape(self):
        """
        Get map shape
        :return: a tuple of shape, i.e. (rows, columns)
        """
        return self.map.shape
132 133

    def __update_path_planning(self):
Yixing Lin's avatar
Yixing Lin committed
134
        occ_threshold = 0.2
135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152
        if self.path_planner is not None and self.update_counter % 5 ==0 and self.update_counter > 5:
            goal = self.supervisor.goal()  # get the goal
            start = self.slam.get_estimated_pose().sunpack() # get the estimated pose from slam
            gx, gy = self.__to_gridmap_position(goal[0], goal[1])
            sx, sy = self.__to_gridmap_position(start[0], start[1])
            self.map[sy, sx] = 0
            if self.map[gy, gx] < occ_threshold:
                bool_map = self.blur(self.map)
               # bool_map = np.copy(self.map)  # calculate a boolean map
                bool_map[bool_map >= occ_threshold] = True
                bool_map[bool_map < occ_threshold] = False
                bool_map = bool_map.astype(np.bool)
                bool_map[sy, sx] = False
                bool_map[gy, gx] = False
                self.path = self.path_planner.planning(sx, sy, gx, gy, bool_map, type='euclidean')
            else:
                self.path = list()

153
    def __inverse_sensor_model(self, points):
154
        """
155
        Inverse sensor model. Calculate the probability the occupied points on a segment
156 157 158 159
        :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.
        """
160
        a = 0.3*self.resolution # the thick of the obstacles
161
        x0, y0 = points[0] # start point position
162 163 164
        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
165 166
        probs = [] # occupancy probabilities of the grids on the segment.
        for xi, yi in points:
167
            r = sqrt((xi - x0) ** 2 + (yi - y0) ** 2)  # a distance of a cell from the robot
168
            prob = self.prob_unknown
169 170 171 172 173
            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:
174 175
                prob = self.prob_free
            probs.append((int(xi), int(yi), prob))
176

177 178
        return probs

179

180
    def blur(self, image):
Yixing Lin's avatar
Yixing Lin committed
181 182 183 184
        """
        Blur the map
        :return:
        """
185
        return gaussian_filter(image, sigma=1)
Yixing Lin's avatar
Yixing Lin committed
186

187 188 189 190 191 192 193 194 195 196 197 198 199 200 201
    def __calc_lines(self, z):
        """
        Calculate the start points end end points of segments.
        :param z: Measurement, represented as tuple of measured distance and measured angle
        :return:
                A list of segments. A single line is represented by (x0, y0, x1, y1)
                    where (x0, y0) is the position of the start point and (x1, y1) is the position of the end point.
        """
        pose = self.slam.get_estimated_pose().sunpack()
        lines = []
        for measurement in z:
            lmx, lmy = self.calc_landmark_position(pose, measurement)
            lines.append((pose[0], pose[1], lmx, lmy))
        return lines

Yixing Lin's avatar
Yixing Lin committed
202 203 204
    def __to_gridmap_position(self, x, y):
        """
        Calculate the position of a pixel in the grid map
205 206
        :param x: x in meters
        :param y: y in meters
Yixing Lin's avatar
Yixing Lin committed
207
        :return:
208 209
                pix_x in pixels
                pix_y in pixels
Yixing Lin's avatar
Yixing Lin committed
210
        """
211 212
        pix_x = round((x + self.offset[0]) * self.resolution)
        pix_y = round((y + self.offset[1]) * self.resolution)
Yixing Lin's avatar
Yixing Lin committed
213 214
        return int(pix_x), int(pix_y)

215 216 217 218 219 220 221 222 223 224 225 226 227
    def __to_world_position(self, x, y):
        """
        Calculate the position of a pixel in the grid map
        :param x: x in pixels
        :param y: y in pixels
        :return:
                pix_x in meters
                pix_y in meters
        """
        meter_x = x / self.resolution - self.offset[0]
        meter_y = y / self.resolution - self.offset[1]
        return meter_x, meter_y

228 229 230 231 232 233 234 235 236 237 238 239 240
    def __prob2log(self, p):
        """
        :param p: probability that a grid is occupied
        :return: log likelihood
        """
        return np.log(p/(1-p))

    def __log2prob(self, lx):
        """
        :param lx: log likelihood
        :return: probability that a grid is occupied
        """
        return 1 - 1/(1+np.exp(lx))
Yixing Lin's avatar
Yixing Lin committed
241

242 243 244 245 246 247 248 249 250 251 252 253
    @staticmethod
    def calc_landmark_position(x, z):
        """
        Returns the measured landmark position
        :param x: The robots pose (or combined state vector, only matters that first three elements are robot pose)
        :param z: Measurement, represented as tuple of measured distance and measured angle
        :return: Measured landmark position
        """
        lm = [0, 0]
        lm[0] = x[0] + z[0] * cos(z[1] + x[2])
        lm[1] = x[1] + z[0] * sin(z[1] + x[2])
        return lm