def point_segment_distance(point, segment):
"""Shortest distance between a point and a segment.
Args:
point (np.ndarray): Point
segment (list): Segment
Returns:
bool: Shortest distance.
"""
dx = segment[1][0] - segment[0][0]
dy = segment[1][1] - segment[0][1]
if dx == dy == 0: # the segment's just a point
return math.hypot(point[0] - segment[0][0], point[1] - segment[0][1])
# Calculate the t that minimizes the distance.
t = ((point[0] - segment[0][0]) * dx +
(point[1] - segment[0][1]) * dy) / (dx * dx + dy * dy)
# See if this represents one of the segment's
# end points or a point in the middle.
if t < 0:
return dist(point, segment[0])
elif t > 1:
return dist(point, segment[1])
else:
return point_perpendicular_distance(point, segment)
def straight_line_distance(first, second):
"""Computes the Straight Line distance between two line segments.
Args:
first (list): First segment
second (list): Second segment
Returns:
float: Straight line distance.
"""
a = dist(first[0], second[0])
b = dist(first[0], second[1])
c = dist(first[1], second[0])
d = dist(first[1], second[1])
distances = []
for s, t in [[first, second], [second, first]]:
distance = (a + b + c + d) / 4
distance += (length(s) + length(t)) / 4
distance -= mod_hausdorff_distance(s, t) / 4
distance += perpendicular_distance(s, t)
distances.append(distance)
return min(distances)
def midpoint_distance(first, second):
"""Computes the Midpoint distance between two line segments.
Args:
first (list): First segment
second (list): Second segment
Returns:
float: Modified Hausdorff distance.
"""
first_mid = (first[0] + first[1]) / 2
second_mid = (second[0] + second[1]) / 2
return min(
dist(first[0], second[0]) + dist(first[1], second[1]),
dist(first[0], second[1]) +
dist(first[1], second[0])) + 3 * dist(first_mid, second_mid)
def ordered_frechet_distance(first, second):
"""Computes the discrete frechet distance between two polygonal lines
Algorithm: http://www.kr.tuwien.ac.at/staff/eiter/et-archive/cdtr9464.pdf
P and Q are arrays of 2-element arrays (points)
Args:
first (list): First segment
second (list): Second segment
Returns:
float: Frechet distance.
"""
a = dist(first[0], second[0])
b = max(dist(first[0], second[1]), a)
c = max(dist(first[1], second[0]), a)
d = max(min(a, b, c), dist(first[1], second[1]))
return d
def drive(self, distance):
"""Move forward a specified distance."""
start = self.robot.adjusted.location
self.move(numpy.sign(distance) * 255, False)
# Keep moving while not moved full distance.
while util.dist(self.robot.adjusted.location, start) < distance:
time.sleep(0.02)
self.move(0, False)
def origin_distance(first, second):
"""Computes the distance between the closest point on each lines to the origin.
Args:
first (list): First segment
second (list): Second segment
Returns:
float: Distance.
"""
first_origin = perpendicular_point(numpy.zeros(2), first)
second_origin = perpendicular_point(numpy.zeros(2), second)
return dist(first_origin, second_origin)
def get_prior_distribution(self):
"""Get the prior normal distribution for position and angle."""
# Calculate change in angle and distance moved.
turned = self.current.adjusted.heading - self.prev.adjusted.heading
distance = util.dist(self.current.adjusted.location,
self.prev.adjusted.location)
# Calculate probability distribution for angle turned.
angle_std = ((abs(turned)) / ANGLE_MEAN) * ANGLE_STDDEV + 5
angle_keys = [
i for i in range(int(-angle_std * 2),
int(angle_std * 2) + 1)
]
angle_values = stats.norm.pdf(angle_keys, 0, angle_std)
angle_probs = {
angle_keys[i]: angle_values[i]
for i in range(len(angle_keys))
}
# Calculate probability distribution for position.
distance_std = (distance / DISTANCE_MEAN) * DISTANCE_STDDEV + 1
distance_distribution = stats.norm(0, distance_std)
position_keys = [
numpy.array([i, j]) for i in range(-3, 4) for j in range(-3, 4)
]
prior_distribution = {}
for pos in position_keys:
for ang in angle_keys:
distance = util.dist(self.current.adjusted.location,
pos + self.current.adjusted.location)
prior_distribution[tuple(pos),
ang] = distance_distribution.pdf(
distance) * angle_probs[ang]
return prior_distribution, angle_keys, position_keys
def recalculate_line(consensus, is_vertical):
"""Given a discovered consensus, recalculate the line with other points that are close enough.
Args:
consensus (list): List of consensus measurements.
is_vertical (bool): Whether the line is almost vertical.
Returns:
tuple: Start and end points of line segment.
"""
cartesian_consensus = numpy.array([point.location for point in consensus])
# If almost vertical, calculate line in terms of y.
if is_vertical:
cartesian_consensus = numpy.fliplr(cartesian_consensus)
# Calculate regression line.
slope, intercept, r_, p_, e_ = stats.linregress(cartesian_consensus[:, 0],
cartesian_consensus[:, 1])
start = util.nearest(cartesian_consensus[0], slope, intercept)
end = util.nearest(cartesian_consensus[0], slope, intercept)
distance = 0
for i in range(len(consensus)):
for j in range(i + 1, len(consensus)):
point_a = util.nearest(cartesian_consensus[i], slope, intercept)
point_b = util.nearest(cartesian_consensus[j], slope, intercept)
new_dist = util.dist(point_a, point_b)
if new_dist > distance:
distance = new_dist
start = point_a
end = point_b
# If line is vertical, flip coordinates back.
if is_vertical:
start = numpy.flipud(start)
end = numpy.flipud(end)
return start, end
def drive(self, distance):
start = self.robot.adjusted.location
self.move(np.sign(distance)*100, False)
while util.dist(self.robot.adjusted.location, start) < distance:
time.sleep(0.02)
self.move(0, False)