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])
