def observation_update(self, left_lane_image, right_lane_image):
"""Update particle weights using observation
:param left_lane_image: an array of line points for the left lane in the image, e.g. [u1, v1, u2, v2]
:param right_lane_image: an array of line points for the right lane in the image, e.g. [u1, v1, u2, v2]
:return:
"""
if left_lane_image is None and right_lane_image is None:
# No lanes detected
return
elif right_lane_image is None:
# Only left lane is detected
left_lane_ground = functions.line_image2ground(left_lane_image)
left_slope, left_intercept = functions.line_points2slope_intercept(left_lane_ground)
y_expected = (self.lane_width / 2.) - left_intercept
left_angle = np.arctan(left_slope)
phi_expected = -left_angle
elif left_lane_image is None:
# Only right lane detected
right_lane_ground = functions.line_image2ground(right_lane_image)
right_slope, right_intercept = functions.line_points2slope_intercept(right_lane_ground)
y_expected = (-self.lane_width / 2.) + right_intercept
right_angle = np.arctan(right_slope)
phi_expected = -right_angle
else:
# Both lanes are detected
left_lane_ground = functions.line_image2ground(left_lane_image)
right_lane_ground = functions.line_image2ground(right_lane_image)
# Determine Duckiebot pose from observation
left_slope, left_intercept = functions.line_points2slope_intercept(left_lane_ground)
right_slope, right_intercept = functions.line_points2slope_intercept(right_lane_ground)
# Expected y position is the deviation from the centre of the left and right intercepts
y_expected = -(left_intercept + right_intercept) / 2.
# Convert slopes to angles
left_angle = np.arctan(left_slope)
right_angle = np.arctan(right_slope)
# Expected angle is the negative of the average of the left and right slopes
phi_expected = -((left_angle + right_angle) / 2.)
# Compare the position of each particle with the expected position from the observation to adjust weight
for particle in self.particles:
y_diff = np.abs(particle.y - y_expected)
phi_diff = np.abs(particle.phi - phi_expected)
# This isn't a good way of determining likelihood, but it seems to be work
y_likelihood = max(1. - y_diff / 0.5, 0.1)
phi_likelihood = max(1. - phi_diff / (np.pi / 4.), 0.1)
particle.weight *= y_likelihood * phi_likelihood
def estimate_pose(left_lane_image, right_lane_image):
# Estimate pose from a single observation
global lane_width
if right_lane_image is None:
# Only left lane is detected
left_lane_ground = functions.line_image2ground(left_lane_image)
left_slope, left_intercept = functions.line_points2slope_intercept(
left_lane_ground)
y_expected = (lane_width / 2.) - left_intercept
left_angle = np.arctan(left_slope)
phi_expected = -left_angle
elif left_lane_image is None:
# Only right lane detected
right_lane_ground = functions.line_image2ground(right_lane_image)
right_slope, right_intercept = functions.line_points2slope_intercept(
right_lane_ground)
y_expected = (-lane_width / 2.) + right_intercept
right_angle = np.arctan(right_slope)
phi_expected = -right_angle
else:
# Both lanes are detected
left_lane_ground = functions.line_image2ground(left_lane_image)
right_lane_ground = functions.line_image2ground(right_lane_image)
# Determine Duckiebot pose from observation
left_slope, left_intercept = functions.line_points2slope_intercept(
left_lane_ground)
right_slope, right_intercept = functions.line_points2slope_intercept(
right_lane_ground)
# Expected y position is the deviation from the centre of the left and right intercepts
y_expected = -(left_intercept + right_intercept) / 2.
# Convert slopes to angles
left_angle = np.arctan(left_slope)
right_angle = np.arctan(right_slope)
# Expected angle is the negative of the average of the left and right slopes
phi_expected = -((left_angle + right_angle) / 2.)
return [y_expected, phi_expected]