def checkIntersect(pos, wp): # for wall in walls: # if intersect.doIntersect(line, wall): # return False if intersect.get_intersect(pos[0], pos[1], wp[0], wp[1]) < 1: return False return True
def checkLine(line, walls): # for wall in walls: # if intersect.doIntersect(line, wall): # return False if intersect.get_intersect(line[0], line[1], line[2], line[3]) < 1: return False return True
def get_velocity(video_name, data, lines, linenames, dist): left = [] right = [] hor = [] for i in xrange(linenames.shape[0]): if linenames[i][0] == 'l': left.append(i) elif linenames[i][0] == 'r': right.append(i) else: hor.append(i) left = np.array(left) right = np.array(right) hor = np.array(hor) collision = np.zeros((data.shape[0], )) velocity = np.inf * np.ones((data.shape[0], )) side, collision = get_intersect(video_name, lines, linenames=linenames) index = np.where(side == 0)[0] if len(index) > 0: velocity[index] = get_velocity_side(data[index], collision[index], 0, lines[right], linenames[right], right, dist, find_last_frame) index = np.where(side == 1)[0] if len(index) > 0: velocity[index] = get_velocity_side(data[index], collision[index], -1, lines[left], linenames[left], left, dist, find_last_frame) index = np.where(side == 2)[0] if len(index) > 0: velocity[index] = get_velocity_side(data[index], collision[index], -1, lines[hor], linenames[hor], hor, dist, find_last_frame) num_obj = int(np.max(data[:, 2])) now = np.inf * np.ones((num_obj + 1, )) for idx, obj in enumerate(data): obj_id = int(obj[2]) if (velocity[idx] == np.inf): if now[obj_id] == np.inf: j = idx + 1 while (j < data.shape[0]): if (int(data[j, 2]) == obj_id) and (velocity[j] != np.inf): now[obj_id] = velocity[j] break j = j + 1 velocity[idx] = now[obj_id] else: now[obj_id] = velocity[idx] velocity = np.abs(velocity) return side, velocity, collision
def checkIntersect(pos, wp): # intersect.get_intersect has a vector as parameter, defined by two position # returns 2 if no intersection with walls # returns value between 0 and 1 otherwise # The value is the percentage of overall the vector length where the first intersection occurred if intersect.get_intersect(pos[0], pos[1], wp[0], wp[1]) < 1: return False return True
def perceptionModel(particle): """ :param particle: the particle of which we want to estimate the measurements :return: sse: sum of square error between the actual measurements and the estimated ones (weight of the particle) """ global basePos, directions, laser_offset, laser_max_range sensor_pos = [] laser_end_pos = [] # x_sens and y_sens are the coordinates of the starting point of the i-th group of sensors in the current particle. # Note: Because of our basePos (laser base positions) are calculated from the center of the robot and our particle is # positioned in center of the wheels, in order to estimate the position of the laser in the current particle we # need to add a 0.255 (distance between the center of the wheels and the center of the wheelchair on the x-axis). for i in range(len(basePos)): x_sens = particle[0] + np.cos(particle[2]) * ( basePos[i][0] + 0.255) - np.sin(particle[2]) * basePos[i][1] y_sens = particle[1] + np.sin(particle[2]) * ( basePos[i][0] + 0.255) + np.cos(particle[2]) * basePos[i][1] sensor_pos.append([x_sens, y_sens]) # x_end and y_end are the coordinates of the ending point of the j-th laser sensor of the i-th group for j in range(4): x_end = x_sens + laser_max_range * np.cos( particle[2] + directions[i] + j * laser_offset) y_end = y_sens + laser_max_range * np.sin( particle[2] + directions[i] + j * laser_offset) laser_end_pos.append([x_end, y_end]) laser_values = [] # Evaluating "intersect" of the lasers with the walls of the map in the current particle for i in range(len(sensor_pos)): for j in range(4): value = intersect.get_intersect(sensor_pos[i][0], sensor_pos[i][1], laser_end_pos[4 * i + j][0], laser_end_pos[4 * i + j][1]) if value == 2: laser_values.append(laser_max_range) else: laser_values.append(value * laser_max_range) # Calculating the SSE sse = np.sum(np.square(distance - laser_values)) """ Used to verify that the lasers are correctly represented in each particle clearLines("path") configureLines("path", 5, 0.2, 0.8, 0.2) for i in range(len(sensor_pos)): for j in range(4): appendLines("path", sensor_pos[i][0], sensor_pos[i][1], 0.5) appendLines("path", laser_end_pos[4*i+j][0], laser_end_pos[4*i+j][1], 0.5) """ return sse
def generate_measurement(particle, walls, debug=False, color=1): m = [] d = particle[2] cos = math.cos(d) sin = math.sin(d) r = np.array([[cos, -sin], [sin, cos]]) # Laser scanner positions fl, fr, bl, br basePos = np.array([[ 0.320234, 0.230233], [ 0.320234, -0.230234], [-0.306395, 0.214315], [-0.306395, -0.214316]]) directions = np.array([0.262, -1.309, 1.833, -2.879]) np.add(directions, d, directions) for i in range(4): p = np.array(basePos[i]) p = np.dot(r, p) p[0] += particle[0] p[1] += particle[1] rOffset = 0. for l in range(4): p2 = np.array([math.cos(directions[i]+rOffset), math.sin(directions[i]+rOffset)]) np.multiply(p2, 4., p2) np.add(p, p2, p2) line = np.append(p, p2) l1 = intersect.get_intersect(line[0], line[1], line[2], line[3]) l1 = np.min([l1, 1.0]) if debug: np.subtract(p2, p, p2) np.multiply(p2, l1, p2) np.add(p2, p, p2) if color == 1: appendLines("debugRays", p[0], p[1], 0.35) appendLines("debugRays", p2[0], p2[1], 0.35) else: appendLines("debugRays2", p[0], p[1], 0.35) appendLines("debugRays2", p2[0], p2[1], 0.35) m.append(l1*4.) rOffset += 0.349 return np.array(m)
def checkDistance(pos, wp): #print "lines: " + str(pos) + ", " + str(wp) return intersect.get_intersect(pos[0], pos[1], wp[0], wp[1])