def __init__(self,
                 bounds=(-10.0, 10.0, -10.0, 10.0),
                 resolution=0.2,
                 prior=0.5,
                 unknown=50.0,
                 decay=0.00001,
                 num_angles=1):
        """ bounds: the extends of the map, in m
        resolution: the size of each grid cell, in m """

        assert (bounds[1] > bounds[0])
        assert (bounds[3] > bounds[2])
        assert (resolution > 0)

        numx = int(math.ceil((bounds[1] - bounds[0]) / resolution))
        numy = int(math.ceil((bounds[3] - bounds[2]) / resolution))

        tot_prod = int(numx * numy * num_angles)

        self.shape = (numy, numx, num_angles)
        self.bounds = bounds
        self.resolution = resolution
        self.unknown = unknown
        big_covar = unknown  #1000.0
        small_covar = decay  #10^-5
        p_occ = logodds(prior)
        # C, R matrix will be changed for every measurement, these
        # are just place holder values

        x0 = p_occ * numpy.ones((tot_prod, 1))
        P0 = big_covar * numpy.ones((tot_prod, 1))
        Q = small_covar * numpy.ones((tot_prod, 1))
        R = numpy.ones((tot_prod, 1))
        self.kf = KalmanSmootherFast(x0=x0, P0=P0, Q=Q, R=R)
    def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2,
                 prior=0.5, unknown=50.0, decay=0.00001, num_angles=1):
        """ bounds: the extends of the map, in m
        resolution: the size of each grid cell, in m """

        assert(bounds[1] > bounds[0])
        assert(bounds[3] > bounds[2])
        assert(resolution > 0)

        numx = int(math.ceil((bounds[1] - bounds[0])/resolution))
        numy = int(math.ceil((bounds[3] - bounds[2])/resolution))
        
        tot_prod = int(numx*numy*num_angles)
        
        self.shape = (numy, numx, num_angles)
        self.bounds = bounds
        self.resolution = resolution
        self.unknown = unknown
        big_covar = unknown #1000.0
        small_covar = decay #10^-5
        p_occ = logodds(prior)
        # C, R matrix will be changed for every measurement, these
        # are just place holder values

        x0 = p_occ*numpy.ones((tot_prod,1))
        P0 = big_covar*numpy.ones((tot_prod,1))
        Q = small_covar*numpy.ones((tot_prod,1))
        R = numpy.ones((tot_prod,1))
        self.kf = KalmanSmootherFast(x0=x0, P0=P0, Q=Q, R=R)
class OccupancyGrid(object):
    """ Occupancy grid type map """
    def __init__(self,
                 bounds=(-10.0, 10.0, -10.0, 10.0),
                 resolution=0.2,
                 prior=0.5,
                 unknown=50.0,
                 decay=0.00001,
                 num_angles=1):
        """ bounds: the extends of the map, in m
        resolution: the size of each grid cell, in m """

        assert (bounds[1] > bounds[0])
        assert (bounds[3] > bounds[2])
        assert (resolution > 0)

        numx = int(math.ceil((bounds[1] - bounds[0]) / resolution))
        numy = int(math.ceil((bounds[3] - bounds[2]) / resolution))

        tot_prod = int(numx * numy * num_angles)

        self.shape = (numy, numx, num_angles)
        self.bounds = bounds
        self.resolution = resolution
        self.unknown = unknown
        big_covar = unknown  #1000.0
        small_covar = decay  #10^-5
        p_occ = logodds(prior)
        # C, R matrix will be changed for every measurement, these
        # are just place holder values

        x0 = p_occ * numpy.ones((tot_prod, 1))
        P0 = big_covar * numpy.ones((tot_prod, 1))
        Q = small_covar * numpy.ones((tot_prod, 1))
        R = numpy.ones((tot_prod, 1))
        self.kf = KalmanSmootherFast(x0=x0, P0=P0, Q=Q, R=R)

    def __call__(self, sonar_measurement):
        indices = sonar_measurement.indices
        # Convert probabilities to log odds format first
        prob = logodds(sonar_measurement.prob)
        prob = prob[:, numpy.newaxis]
        n = sonar_measurement.var.shape[0]
        R = numpy.reshape(numpy.asarray(sonar_measurement.var), (n, 1))
        ind = indices[:, 2] * self.shape[1] * self.shape[
            0] + indices[:, 1] * self.shape[0] + indices[:, 0]

        return self.kf.meas_update(prob, ind, R)

    def eval_measurement(self, cells, dist):
        """ Evaluate measurements against existing map """
        def ownpdf(x, m, sigma):
            """ Non-normalized pdf for cell/sensor measurement 
            x - eval points
            m - mean, either scaler or same shape as x
            sigma - 'width' of distribution """

            pdf = numpy.zeros(numpy.shape(x))

            top_ind = (abs(x - m) <= sigma / 2.0)
            if (top_ind.any()):
                pdf[top_ind] = 1.0
            interp_ind = (abs(x - m) > sigma / 2.0) & (abs(x - m) < sigma)
            if (interp_ind.any()):
                pdf[interp_ind] = (1.0 - (abs(x - m)[interp_ind] - sigma / 2) /
                                   (sigma / 2))
            return pdf

        num_angles = self.shape[2]
        cells.sort(order='r', axis=0)
        tmp = (cells['a_w'] + math.pi) * num_angles / (2.0 * math.pi)
        tmp = numpy.floor(tmp)
        # Wrap-around
        tmp[tmp >= num_angles] = num_angles - 1
        tmp = numpy.reshape(tmp, (-1, 1))
        indices = numpy.concatenate((cells['opt'], tmp), 1)
        ind = indices[:, 2] * self.shape[1] * self.shape[
            0] + indices[:, 1] * self.shape[0] + indices[:, 0]
        ind = ind.astype(int)
        pdf = ownpdf(cells['r'], dist, self.resolution)
        prob_occ = inv_logodds(self.kf.x_new[ind]).ravel()

        #multiplier = 1.0
        #prob = 0
        #for j in range(numpy.shape(cells)[0]):
        #    prob += multiplier*prob_occ[j]*pdf[j]
        #    multiplier *= (1 - prob_occ[j])

        # Same calculations as for loop above, but much more efficient
        multiplier = numpy.cumprod(1 - prob_occ)
        multiplier = numpy.roll(multiplier, 1)
        multiplier[0] = 1.0
        prob = numpy.dot(multiplier, prob_occ * pdf)
        return prob

    def time_update(self):
        self.kf.time_update()

    def smooth(self, z_next):
        self.kf.smooth(z_next.kf.x_new, z_next.kf.P)

    def get_nav_map(self):

        tmp_prob = numpy.reshape(numpy.asarray(self.kf.x_new),
                                 (-1, self.shape[2]),
                                 order='F')
        tmp_var = numpy.reshape(numpy.asarray(self.kf.P), (-1, self.shape[2]),
                                order='F')

        tmp_prob = numpy.copy(tmp_prob)

        tmp_prob[tmp_var >= self.unknown] = -numpy.Inf
        ind = numpy.argmax(tmp_prob, axis=1)

        tmp_prob = tmp_prob[numpy.arange(tmp_prob.shape[0]), ind]
        tmp_var = tmp_var[numpy.arange(tmp_var.shape[0]), ind]

        tmp_prob = tmp_prob.reshape((self.shape[0], self.shape[1]))
        tmp_var = tmp_var.reshape((self.shape[0], self.shape[1]))

        return (inv_logodds(tmp_prob), tmp_var)

    def find_visible_cells(self, cone):
        """ Finds all cells centers of all grids within the cone specified
        state - (x,y,dir)
        prec - FOV width, summetric around dir
        dist - max distance to search """
        def course_crop(dim, bounds, cone):
            """ Rectangular bounds on which indices need to be evaluated """
            (xmin, xmax, ymin, ymax) = cone.get_bounding_rect()

            xleft = math.floor(
                (dim[1] - 1) * (xmin - bounds[0]) / (bounds[1] - bounds[0]))
            xright = math.ceil(
                (dim[1] - 1) * (xmax - bounds[0]) / (bounds[1] - bounds[0]))

            xleft = max((xleft, 1))
            xright = min((xright, dim[1] - 1))

            yupp = math.floor(
                (dim[0] - 1) * (ymin - bounds[2]) / (bounds[3] - bounds[2]))
            ylow = math.ceil(
                (dim[0] - 1) * (ymax - bounds[2]) / (bounds[3] - bounds[2]))

            yupp = max((yupp, 1))
            ylow = min((ylow, dim[0] - 1))

            return (int(xleft), int(xright), int(yupp), int(ylow))

        (xle, xri, yup, ylo) = course_crop(self.shape, self.bounds, cone)

        # Evaluate each cell center for the FOV
        x_ind = numpy.arange(xle, xri + 1, dtype=numpy.int)
        y_ind = numpy.arange(yup, ylo + 1, dtype=numpy.int)

        x, y = numpy.meshgrid(x_ind * self.resolution + self.bounds[0],
                              y_ind * self.resolution + self.bounds[2])

        x_ind, y_ind = numpy.meshgrid(x_ind, y_ind)

        x = x.reshape((-1, 1))
        y = y.reshape((-1, 1))
        x_ind = x_ind.reshape((-1, 1))
        y_ind = y_ind.reshape((-1, 1))

        if (x.shape[0] == 0):
            return None

        # cells will contains (a,r,y_ind,x_ind) for all visible cells
        cells = cone(y[:, 0], x[:, 0], numpy.hstack([y_ind, x_ind]))

        if (cells == None):
            return None

        cells.sort(order='r', axis=0)

        return cells
class OccupancyGrid(object):
    """ Occupancy grid type map """
    def __init__(self, bounds=(-10.0, 10.0, -10.0, 10.0), resolution=0.2,
                 prior=0.5, unknown=50.0, decay=0.00001, num_angles=1):
        """ bounds: the extends of the map, in m
        resolution: the size of each grid cell, in m """

        assert(bounds[1] > bounds[0])
        assert(bounds[3] > bounds[2])
        assert(resolution > 0)

        numx = int(math.ceil((bounds[1] - bounds[0])/resolution))
        numy = int(math.ceil((bounds[3] - bounds[2])/resolution))
        
        tot_prod = int(numx*numy*num_angles)
        
        self.shape = (numy, numx, num_angles)
        self.bounds = bounds
        self.resolution = resolution
        self.unknown = unknown
        big_covar = unknown #1000.0
        small_covar = decay #10^-5
        p_occ = logodds(prior)
        # C, R matrix will be changed for every measurement, these
        # are just place holder values

        x0 = p_occ*numpy.ones((tot_prod,1))
        P0 = big_covar*numpy.ones((tot_prod,1))
        Q = small_covar*numpy.ones((tot_prod,1))
        R = numpy.ones((tot_prod,1))
        self.kf = KalmanSmootherFast(x0=x0, P0=P0, Q=Q, R=R)

    def __call__(self, sonar_measurement):
        indices = sonar_measurement.indices
        # Convert probabilities to log odds format first
        prob = logodds(sonar_measurement.prob)
        prob = prob[:, numpy.newaxis]
        n = sonar_measurement.var.shape[0]
        R = numpy.reshape(numpy.asarray(sonar_measurement.var),(n,1))
        ind = indices[:, 2]*self.shape[1]*self.shape[0] + indices[:, 1]*self.shape[0]+indices[:, 0]

        return self.kf.meas_update(prob, ind, R)
    
    def eval_measurement(self, cells, dist):
        """ Evaluate measurements against existing map """
        
        def ownpdf(x, m, sigma):
            """ Non-normalized pdf for cell/sensor measurement 
            x - eval points
            m - mean, either scaler or same shape as x
            sigma - 'width' of distribution """

            pdf = numpy.zeros(numpy.shape(x))

            top_ind = (abs(x-m) <= sigma/2.0)
            if (top_ind.any()):
                pdf[top_ind] = 1.0
            interp_ind = (abs(x-m) > sigma/2.0) & (abs(x-m) < sigma)
            if (interp_ind.any()):
                pdf[interp_ind] = (
                       1.0 - (abs(x-m)[interp_ind]-sigma/2)/(sigma/2)
                       ) 
            return pdf
        
        num_angles = self.shape[2]
        cells.sort(order='r', axis=0)
        tmp = (cells['a_w'] + math.pi)*num_angles/(2.0*math.pi)
        tmp = numpy.floor(tmp)
        # Wrap-around
        tmp[tmp >= num_angles] = num_angles-1
        tmp = numpy.reshape(tmp,(-1,1))
        indices = numpy.concatenate((cells['opt'], tmp), 1)
        ind = indices[:, 2]*self.shape[1]*self.shape[0] + indices[:, 1]*self.shape[0]+indices[:, 0]
        ind = ind.astype(int)
        pdf = ownpdf(cells['r'], dist, self.resolution)
        prob_occ = inv_logodds(self.kf.x_new[ind]).ravel()
        
        #multiplier = 1.0
        #prob = 0
        #for j in range(numpy.shape(cells)[0]):
        #    prob += multiplier*prob_occ[j]*pdf[j]
        #    multiplier *= (1 - prob_occ[j])
        
        # Same calculations as for loop above, but much more efficient
        multiplier = numpy.cumprod(1 - prob_occ)
        multiplier = numpy.roll(multiplier, 1)
        multiplier[0] = 1.0
        prob = numpy.dot(multiplier, prob_occ*pdf)
        return prob
    
    def time_update(self):
        self.kf.time_update()
        
    def smooth(self, z_next):
        self.kf.smooth(z_next.kf.x_new, z_next.kf.P)
    
    def get_nav_map(self):
        
        tmp_prob = numpy.reshape(numpy.asarray(self.kf.x_new),
                                 (-1,self.shape[2]), order='F')
        tmp_var = numpy.reshape(numpy.asarray(self.kf.P),
                                (-1,self.shape[2]), order='F')
        
        tmp_prob = numpy.copy(tmp_prob)
        
        tmp_prob[tmp_var >= self.unknown] = -numpy.Inf
        ind = numpy.argmax(tmp_prob,axis=1)
        
        tmp_prob = tmp_prob[numpy.arange(tmp_prob.shape[0]), ind]
        tmp_var = tmp_var[numpy.arange(tmp_var.shape[0]), ind]
        
        tmp_prob = tmp_prob.reshape((self.shape[0],self.shape[1]))
        tmp_var = tmp_var.reshape((self.shape[0],self.shape[1]))
        
        return (inv_logodds(tmp_prob), tmp_var)
    
    def find_visible_cells(self, cone):
        """ Finds all cells centers of all grids within the cone specified
        state - (x,y,dir)
        prec - FOV width, summetric around dir
        dist - max distance to search """
        
        def course_crop(dim, bounds, cone):
            """ Rectangular bounds on which indices need to be evaluated """
            (xmin, xmax, ymin, ymax) = cone.get_bounding_rect()

            xleft = math.floor((dim[1]-1)*
                               (xmin-bounds[0])/(bounds[1]-bounds[0]))
            xright = math.ceil((dim[1]-1)*
                               (xmax-bounds[0])/(bounds[1]-bounds[0]))
            
            xleft = max((xleft, 1))
            xright = min((xright, dim[1]-1))
    
            yupp = math.floor((dim[0]-1)*
                              (ymin-bounds[2])/(bounds[3]-bounds[2]))
            ylow = math.ceil((dim[0]-1)*
                             (ymax-bounds[2])/(bounds[3]-bounds[2]))
            
            yupp = max((yupp, 1))
            ylow = min((ylow, dim[0]-1))

            return (int(xleft), int(xright), int(yupp), int(ylow))

        
        (xle, xri, yup, ylo) = course_crop(self.shape, self.bounds, cone)

        # Evaluate each cell center for the FOV
        x_ind = numpy.arange(xle, xri+1, dtype=numpy.int)
        y_ind = numpy.arange(yup, ylo+1, dtype=numpy.int)
        
        x, y = numpy.meshgrid(x_ind*self.resolution+self.bounds[0],
                              y_ind*self.resolution+self.bounds[2])

        x_ind, y_ind = numpy.meshgrid(x_ind, y_ind)
        
        x = x.reshape((-1, 1))
        y = y.reshape((-1, 1))
        x_ind = x_ind.reshape((-1, 1))
        y_ind = y_ind.reshape((-1, 1))
        
        if (x.shape[0] == 0):
            return None
            
        # cells will contains (a,r,y_ind,x_ind) for all visible cells
        cells  = cone(y[:, 0], x[:, 0], numpy.hstack([y_ind, x_ind]))
        
        if (cells == None):
            return None

        cells.sort(order='r', axis=0)

        return cells