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_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 __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_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 assign_cylinders(cylinders, robot_pose, scanner_displacement,
reference_cylinders):
# 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])
# Find closest cylinders.
result = []
for c in cylinders:
# Get world coordinate of cylinder.
x, y = LegoLogfile.scanner_to_world(scanner_pose, c[2:4])
# Find closest cylinder in reference cylinder set.
best_dist_2 = 1e300
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((c[0:2], best_ref))
return result
def assign_cylinders(cylinders, robot_pose, scanner_displacement,
reference_cylinders):
# 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])
# Find closest cylinders.
result = []
for c in cylinders:
# Get world coordinate of cylinder.
x, y = LegoLogfile.scanner_to_world(scanner_pose, c[2:4])
# Find closest cylinder in reference cylinder set.
best_dist_2 = 1e300
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((c[0:2], best_ref))
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_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
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
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.
# Plot a scan of the robot using matplotlib.
# 03_a_plot_scan
# XU Shang, 2016-10-13
from pylab import plot,show
from lego_robot import LegoLogfile
# Read the logfile which contains all scans.
logfile = LegoLogfile()
logfile.read("robot4_scan.txt")
# Plot one scan.
plot(logfile.scan_data[10])
show()
else:
g1 = x + l * cos(theta)
g2 = y + l * sin(theta)
g3 = theta
return array([g1, g2, g3])
if __name__ == '__main__':
scanner_displacement = 30.0 # scanner's position related to robot's coordinate
ticks_to_mm = 0.349 # convert ticks from motor to distance
robot_width = 155.0
# initial state
state = array([1850.0, 1897.0, 213.0 / 180 * pi])
# read data from motor's tick sensor
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
states = []
# loop over all motor ticks to generate a states list
for ticks in logfile.motor_ticks:
control = array(ticks) * ticks_to_mm
state = ExtendedKalmanFilter.g(state, control, robot_width)
states.append(state)
f = open("states_from_ticks.txt", "w")
# output the state of scanner instead of the robot
for s in states:
print >> f, "F %f %f %f" % \
tuple(s + [scanner_displacement*cos(s[2]), scanner_displacement*sin(s[2]),0.0])
# Plot the increments of the left and right motor.
# 01_c_plot_motor_increments.py
# Claus Brenner, 07 NOV 2012
from pylab import *
from lego_robot import LegoLogfile
if __name__ == '__main__':
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
plot(logfile.motor_ticks)
show()
if __name__ == '__main__':
# The constants we used for the filter_step.
scanner_displacement = 30.0
ticks_to_mm = 0.349
robot_width = 173.0
# The constants we used for the cylinder detection in our scan.
minimum_valid_distance = 20.0
depth_jump = 100.0
cylinder_offset = 90.0
# The start pose we obtained miraculously.
pose = (1850.0, 1897.0, 3.717551306747922)
# Read the logfile which contains all scans.
logfile = LegoLogfile()
logfile.read("robot4_motors.txt")
logfile.read("robot4_scan.txt")
# Iterate over all positions.
out_file = file("project_landmarks.txt", "w")
for i in xrange(len(logfile.scan_data)):
# Compute the new pose.
pose = filter_step(pose, logfile.motor_ticks[i], ticks_to_mm,
robot_width, scanner_displacement)
# Extract cylinders, also convert them to world coordinates.
cartesian_cylinders = compute_scanner_cylinders(
logfile.scan_data[i], depth_jump, minimum_valid_distance,
cylinder_offset)
world_cylinders = [
