Exemplo n.º 1
0
    def _increase_grid(self, out_of_bounds_pos):
        """
        Extends the grid so the row,col coordinates in out_of_bounds_pos
        fall within the grid.
        """
        # get index of block that needs to be added or blocks to keep rectangular shape
        sign_row = math.copysign(1, out_of_bounds_pos[0])
        sign_col = math.copysign(1, out_of_bounds_pos[1])
        new_pos = (
            int(sign_row * CELLS_PER_BLOCK * math.ceil(abs(sign_row + out_of_bounds_pos[0]) / CELLS_PER_BLOCK)),
            int(sign_col * CELLS_PER_BLOCK * math.ceil(abs(sign_col + out_of_bounds_pos[1]) / CELLS_PER_BLOCK)),
        )
        new_pos = tadd(self.minrange, new_pos)

        current_size = self.grid.shape
        new_minrange = tmin(self.minrange, new_pos)
        new_maxrange = tmax(self.maxrange, new_pos)

        new_size = tsub(new_maxrange, new_minrange)
        offset = tsub(self.minrange, new_minrange)
        offset_end = tadd(offset, current_size)
        grid = np.zeros(shape=new_size)
        grid[offset[0] : offset_end[0], offset[1] : offset_end[1]] = self.grid

        self.grid = grid
        self.minrange = new_minrange
        self.maxrange = new_maxrange
Exemplo n.º 2
0
def calc_weight(measurements, pose, map_):
    probability = 0
    # return probability

    for sensor_angle, measured_dist in _measurement_per_angle(measurements):
        # TODO: Pose of sensor is simplified to pose of robot
        sensor_pose = Pose(pose.x, pose.y, pose.theta + sensor_angle)

        # TODO: what if no expected distance is known? Fixed -> weight

        """likelihoods fields range finder, p172"""
        if measured_dist is not None and measured_dist < MAX_RANGE:  # discard max range readings

            # coordinates of endpoint measured distance
            x_co = sensor_pose.x + measured_dist * math.cos(sensor_pose.theta)
            y_co = sensor_pose.y + measured_dist * math.sin(sensor_pose.theta)

            nearest_object = find_nearest_neighbor(map_, (x_co, y_co))
            # when sensor measurement falls into unknown category, prob is assumed constant p173
            if nearest_object == "unknown":
                # cell outside map
                # cell prob = 0.5
                # ==> measurement in unknown region
                probability += 0.2
                None

            # find nearest neighbour that is occupied
            elif nearest_object is not None:
                # calculate Euclidean distance between measured coord and closest occupied coord
                sub = tsub((x_co, y_co), nearest_object)
                distance = math.hypot(sub[0], sub[1])
                # prob = _prob_of_distances(distance, 0.0)
                if distance < 5:
                    # distance to closest object under threshold
                    probability += 1
                else:
                    # distance to closest object to far away
                    probability += 0

            elif nearest_object is None:
                # no object with prob > 0.9 around the cone top
                probability += 0

        else:
            # sensor got out of range value (short reading or free region)
            probability += 1


    #  calculate the region around the pose, if region is free this pose is more possible
    grid_index = map_.cartesian2grid(pose.x, pose.y)
    weight = 0
    # look in a range around the sampled pose
    RANGE = 4
    for i in range(-RANGE, RANGE + 1, 1):
        for j in range(-RANGE, RANGE + 1, 1):
            pos = (grid_index[0] + i, grid_index[1] + j)
            weight += calc_weight_pose(map_, pos)

    # weight of the sensor measurement
    # weight of the map correspondence
    MAP_POSE_FACTOR = 1
    MEASUREMENT_FACTOR = 30

    # do something with weight
    part1 = probability*MEASUREMENT_FACTOR
    part2 = weight*MAP_POSE_FACTOR
    print('part1 : {0}   part2: {1}'.format(part1, part2))
    return part1 + part2
Exemplo n.º 3
0
def calc_weight(measurements, pose, map_):
    probability = 0
    # return probability

    for sensor_angle, measured_dist in _measurement_per_angle(measurements):
        # TODO: Pose of sensor is simplified to pose of robot
        sensor_pose = Pose(pose.x, pose.y, pose.theta + sensor_angle)

        # TODO: what if no expected distance is known? Fixed -> weight
        """likelihoods fields range finder, p172"""
        if measured_dist is not None and measured_dist < MAX_RANGE:  # discard max range readings

            # coordinates of endpoint measured distance
            x_co = sensor_pose.x + measured_dist * math.cos(sensor_pose.theta)
            y_co = sensor_pose.y + measured_dist * math.sin(sensor_pose.theta)

            nearest_object = find_nearest_neighbor(map_, (x_co, y_co))
            # when sensor measurement falls into unknown category, prob is assumed constant p173
            if nearest_object == "unknown":
                # cell outside map
                # cell prob = 0.5
                # ==> measurement in unknown region
                probability += 0.2
                None

            # find nearest neighbour that is occupied
            elif nearest_object is not None:
                # calculate Euclidean distance between measured coord and closest occupied coord
                sub = tsub((x_co, y_co), nearest_object)
                distance = math.hypot(sub[0], sub[1])
                # prob = _prob_of_distances(distance, 0.0)
                if distance < 5:
                    # distance to closest object under threshold
                    probability += 1
                else:
                    # distance to closest object to far away
                    probability += 0

            elif nearest_object is None:
                # no object with prob > 0.9 around the cone top
                probability += 0

        else:
            # sensor got out of range value (short reading or free region)
            probability += 1

    #  calculate the region around the pose, if region is free this pose is more possible
    grid_index = map_.cartesian2grid(pose.x, pose.y)
    weight = 0
    # look in a range around the sampled pose
    RANGE = 4
    for i in range(-RANGE, RANGE + 1, 1):
        for j in range(-RANGE, RANGE + 1, 1):
            pos = (grid_index[0] + i, grid_index[1] + j)
            weight += calc_weight_pose(map_, pos)

    # weight of the sensor measurement
    # weight of the map correspondence
    MAP_POSE_FACTOR = 1
    MEASUREMENT_FACTOR = 30

    # do something with weight
    part1 = probability * MEASUREMENT_FACTOR
    part2 = weight * MAP_POSE_FACTOR
    print('part1 : {0}   part2: {1}'.format(part1, part2))
    return part1 + part2