def __init__(self,
scan,
cylinders,
points_per_sector=66,
min_distance=300,
inline_threshold=45,
attempts=10,
valid_threshold=0.8):
self.points_per_sector = points_per_sector
self.scan = []
self.sector_rays = []
self.min_distance = min_distance
self.n_valid_sectors = 0
self.valid_sectors = []
self.best_lines = []
self.inline_threshold = inline_threshold
self.landmarks = []
self.walls = []
offset = 0
self.sectors = []
while offset < len(scan):
bearing = LegoLogfile.beam_index_to_angle(offset)
x, y = 3000 * cos(bearing), 3000 * sin(bearing)
self.sector_rays.append(np.array([y / x, -1, 0, 0, 0, x, y]))
ray_r = np.array([y / x, -1, 0])
bearing = LegoLogfile.beam_index_to_angle(offset +
self.points_per_sector)
x, y = 3000 * cos(bearing), 3000 * sin(bearing)
ray_l = np.array([y / x, -1, 0])
sec_points = self.get_sector_scans_without_landmarks(
scan, cylinders, offset)
self.scan += sec_points
# for point in sec_points:
# self.scan.append(point)
offset += self.points_per_sector
sector = Sector(sec_points, (ray_l, ray_r),
inline_threshold=self.inline_threshold,
attempts=attempts,
valid_threshold=valid_threshold)
if sector.valid:
self.n_valid_sectors += 1
self.sectors.append(sector)
self.scan_len = len(self.scan)
# print(self.n_valid_sectors)
bearing = LegoLogfile.beam_index_to_angle(offset)
x, y = 3000 * cos(bearing), 3000 * sin(bearing)
self.sector_rays.append(np.array([y / x, -1, 0, 0, 0, x, y]))
def get_observations(scan, jump, min_dist, cylinder_offset, robot_pose,
scanner_displacement, reference_cylinders,
max_reference_distance):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
# Compute scanner pose from robot pose.
scanner_pose = (robot_pose[0] + cos(robot_pose[2]) * scanner_displacement,
robot_pose[1] + sin(robot_pose[2]) * scanner_displacement,
robot_pose[2])
# For every detected cylinder which has a closest matching pole in the
# reference cylinders set, put the measurement (distance, angle) and the
# corresponding reference cylinder into the result list.
result = []
for c in cylinders:
# Compute the angle and distance measurements.
angle = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in world coordinates.
x, y = distance * cos(angle), distance * sin(angle)
x, y = LegoLogfile.scanner_to_world(scanner_pose, (x, y))
# Find closest cylinder in reference cylinder set.
best_dist_2 = max_reference_distance * max_reference_distance
best_ref = None
for ref in reference_cylinders:
dx, dy = ref[0] - x, ref[1] - y
dist_2 = dx * dx + dy * dy
if dist_2 < best_dist_2:
best_dist_2 = dist_2
best_ref = ref
# If found, add to both lists.
if best_ref:
result.append(((distance, angle), best_ref))
return result
def get_observations(scan, jump, min_dist, cylinder_offset,
robot_pose, scanner_displacement,
reference_cylinders, max_reference_distance):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
# Compute scanner pose from robot pose.
scanner_pose = (robot_pose[0] + cos(robot_pose[2]) * scanner_displacement,
robot_pose[1] + sin(robot_pose[2]) * scanner_displacement,
robot_pose[2])
# For every detected cylinder which has a closest matching pole in the
# reference cylinders set, put the measurement (distance, angle) and the
# corresponding reference cylinder into the result list.
result = []
for c in cylinders:
# Compute the angle and distance measurements.
angle = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in world coordinates.
x, y = distance*cos(angle), distance*sin(angle)
x, y = LegoLogfile.scanner_to_world(scanner_pose, (x, y))
# Find closest cylinder in reference cylinder set.
best_dist_2 = max_reference_distance * max_reference_distance
best_ref = None
for ref in reference_cylinders:
dx, dy = ref[0] - x, ref[1] - y
dist_2 = dx * dx + dy * dy
if dist_2 < best_dist_2:
best_dist_2 = dist_2
best_ref = ref
# If found, add to both lists.
if best_ref:
result.append(((distance, angle), best_ref))
return result
def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset,
points_per_scan, max_cylinder_d):
scan_f = filter2(scan)
# der = compute_derivative(scan, min_dist)
# cylinders = find_cylinders(scan, der, jump, min_dist)
der = compute_derivative(scan_f, min_dist)
# der111 = compute_derivative111(scan_f, min_dist)
der2 = compute_derivative111(der, 0)
mul_der=[]
for i in xrange(len(der2)):
mul_der.append(der[i]*abs(der2[i]))
mul_der = filter2(mul_der)
start_stop = []
start_stop = convert_to_start_stop(mul_der, jump)
cylinders = find_cylinders(scan_f, start_stop, jump,
min_dist, points_per_scan, max_cylinder_d)
# der = compute_derivative(scan, min_dist)
# cylinders = find_cylinders(scan, der, jump, min_dist)
result = []
for c in cylinders:
# Compute the angle and distance measurements.
bearing = LegoLogfile.beam_index_to_angle(c[0][0])
distance = c[0][1] + cylinder_offset
# Compute x, y of cylinder in the scanner system.
x, y = distance*cos(bearing), distance*sin(bearing)
result.append( (np.array([distance, bearing]), np.array([x, y]), c[1]))
return result
def get_observations(scan, jump, min_dist, cylinder_offset, robot,
max_cylinder_distance):
# scan = filter1(scan)
scan_f = filter2(scan)
# der = compute_derivative(scan, min_dist)
# cylinders = find_cylinders(scan, der, jump, min_dist)
der = compute_derivative(scan_f, min_dist)
# der111 = compute_derivative111(scan_f, min_dist)
der2 = compute_derivative111(der, 0)
mul_der = []
for i in xrange(len(der2)):
mul_der.append(der[i] * abs(der2[i]))
mul_der = filter2(mul_der)
start_stop = []
start_stop = convert_to_start_stop(mul_der, jump)
cylinders = find_cylinders(scan_f, start_stop, jump, min_dist)
# Compute scanner pose from robot pose.
scanner_pose = (robot.state[0] +
cos(robot.state[2]) * robot.scanner_displacement,
robot.state[1] +
sin(robot.state[2]) * robot.scanner_displacement,
robot.state[2])
# For every detected cylinder which has a closest matching pole in the
# cylinders that are part of the current state, put the measurement
# (distance, angle) and the corresponding cylinder index into the result list.
result = []
for c in cylinders:
# Compute the angle and distance measurements.
angle = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in world coordinates.
xs, ys = distance * cos(angle), distance * sin(angle)
x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys))
# Find closest cylinder in the state.
best_dist_2 = max_cylinder_distance * max_cylinder_distance
best_index = -1
for index in xrange(robot.number_of_landmarks):
pole_x, pole_y = robot.state[3 + 2 * index:3 + 2 * index + 2]
dx, dy = pole_x - x, pole_y - y
dist_2 = dx * dx + dy * dy
if dist_2 < best_dist_2:
best_dist_2 = dist_2
best_index = index
best_index_2 = robot.find_cylinder((distance, angle), float(0.1))
# Always add result to list. Note best_index may be -1.
# print(">>> best_index %d"%best_index)
if (best_index != best_index_2):
print("best_index %d best_index_2 %d" % (best_index, best_index_2))
result.append(((distance, angle), (x, y), (xs, ys), best_index))
return result
def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
result = []
for c in cylinders:
# Compute the angle and distance measurements.
bearing = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in the scanner system.
x, y = distance*cos(bearing), distance*sin(bearing)
result.append( (distance, bearing, x, y) )
return result
def get_cylinders_from_scan(scan, jump, min_dist, cylinder_offset):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
result = []
for c in cylinders:
# Compute the angle and distance measurements.
bearing = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in the scanner system.
x, y = distance * cos(bearing), distance * sin(bearing)
result.append((distance, bearing, x, y))
return result
def compute_cartesian_coordinates(cylinders, cylinder_offset):
result = []
for c in cylinders:
# --->>> Insert here the conversion from polar to Cartesian coordinates.
# c is a tuple (beam_index, range).
# For converting the beam index to an angle, use
# LegoLogfile.beam_index_to_angle(beam_index)
radius = c[1] + cylinder_offset
angle = LegoLogfile.beam_index_to_angle(c[0])
x = radius * cos(angle)
y = radius * sin(angle)
result.append((x, y)) # Replace this by your (x,y)
return result
def get_sector_scans_without_landmarks(self, scan, cylinders, offset):
result = []
for i in xrange(self.points_per_sector):
j = i + offset
distance = scan[j]
save_scan = True
if (distance < self.min_distance):
continue
for measurement, measurement_in_scanner_system, indxs in cylinders:
# print("indxs[0] %d indxs[1] %d" %(indxs[0], indxs[1]))
if (j >= indxs[0] and j <= indxs[1]):
save_scan = False
break
if save_scan:
bearing = LegoLogfile.beam_index_to_angle(j)
x, y = distance * cos(bearing), distance * sin(bearing)
result.append(np.array([x, y]))
return result
def get_observations(scan, jump, min_dist, cylinder_offset, robot,
max_cylinder_distance):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
# Compute scanner pose from robot pose.
scanner_pose = (robot.specific_state[0] +
cos(robot.specific_state[2]) * robot.scanner_displacement,
robot.specific_state[1] +
sin(robot.specific_state[2]) * robot.scanner_displacement,
robot.specific_state[2])
# For every detected cylinder which has a closest matching pole in the
# cylinders that are part of the current state, put the measurement
# (distance, angle) and the corresponding cylinder index into the result list.
result = []
for c in cylinders:
# Compute the angle and distance measurements.
angle = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in world coordinates.
xs, ys = distance * cos(angle), distance * sin(angle)
x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys))
# Find closest cylinder in the state.
best_dist_2 = max_cylinder_distance * max_cylinder_distance
best_index = -1
for index in range(robot.number_of_landmarks):
pole_x, pole_y = robot.specific_state[3 + 2 * index:3 + 2 * index +
2]
dx, dy = pole_x - x, pole_y - y
dist_2 = dx * dx + dy * dy
if dist_2 < best_dist_2:
best_dist_2 = dist_2
best_index = index
# Always add result to list. Note best_index may be -1.
result.append(((distance, angle), (x, y), (xs, ys), best_index))
return result
def get_observations(scan, jump, min_dist, cylinder_offset,
robot,
max_cylinder_distance):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
# Compute scanner pose from robot pose.
scanner_pose = (
robot.state[0] + cos(robot.state[2]) * robot.scanner_displacement,
robot.state[1] + sin(robot.state[2]) * robot.scanner_displacement,
robot.state[2])
# For every detected cylinder which has a closest matching pole in the
# cylinders that are part of the current state, put the measurement
# (distance, angle) and the corresponding cylinder index into the result list.
result = []
for c in cylinders:
# Compute the angle and distance measurements.
angle = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in world coordinates.
xs, ys = distance*cos(angle), distance*sin(angle)
x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys))
# Find closest cylinder in the state.
best_dist_2 = max_cylinder_distance * max_cylinder_distance
best_index = -1
for index in xrange(robot.number_of_landmarks):
pole_x, pole_y = robot.state[3+2*index : 3+2*index+2]
dx, dy = pole_x - x, pole_y - y
dist_2 = dx * dx + dy * dy
if dist_2 < best_dist_2:
best_dist_2 = dist_2
best_index = index
# Always add result to list. Note best_index may be -1.
result.append(((distance, angle), (x, y), (xs, ys), best_index))
return result
max_cylinder_distance):
der = compute_derivative(scan, min_dist)
cylinders = find_cylinders(scan, der, jump, min_dist)
# Compute scanner pose from robot pose.
scanner_pose = (
robot.state[0] + cos(robot.state[2]) * robot.scanner_displacement,
robot.state[1] + sin(robot.state[2]) * robot.scanner_displacement,
robot.state[2])
# For every detected cylinder which has a closest matching pole in the
# cylinders that are part of the current state, put the measurement
# (distance, angle) and the corresponding cylinder index into the result list.
result = []
for c in cylinders:
# Compute the angle and distance measurements.
angle = LegoLogfile.beam_index_to_angle(c[0])
distance = c[1] + cylinder_offset
# Compute x, y of cylinder in world coordinates.
xs, ys = distance*cos(angle), distance*sin(angle)
x, y = LegoLogfile.scanner_to_world(scanner_pose, (xs, ys))
# Find closest cylinder in the state.
best_dist_2 = max_cylinder_distance * max_cylinder_distance
best_index = -1
for index in range(robot.number_of_landmarks):
pole_x, pole_y = robot.state[3+2*index : 3+2*index+2]
dx, dy = pole_x - x, pole_y - y
dist_2 = dx * dx + dy * dy
if dist_2 < best_dist_2:
best_dist_2 = dist_2
best_index = index
# Always add result to list. Note best_index may be -1.