def interpolate_dominate_lines(signals: dict,
interpolations: dict,
mean_slope: float,
lower_bound: int,
upper_bound: int,
horizontal_limit: int,
slope_variance=SLOPE_VARIANCE):
'''
Interpolates lines based on ROI mask and mean_slope
:param signals: dominate signals in image
:param mean_slope: mean of dominate signals
:param lower_bound: int lower y value in image
:param upper_bound: int upper y value in image
:param horizontal_limit int maximum possible x-value
:param slope_variance: acceptable slope variance
'''
try:
for _line in signals:
_slope = signals[_line]['slope']
_offset = signals[_line]['offset']
x1, y1 = signals[_line]['p1']
x2, y2 = signals[_line]['p2']
if not _slope:
continue
abs_slope = abs(_slope)
delta = abs(abs_slope - mean_slope)
if delta < slope_variance:
logger.debug('%s\t %s', (x1, y1), (x2, y2))
assert _slope != 0, 'slope is zero in signals'
new_p1 = (int((lower_bound - _offset) / _slope), lower_bound)
new_p2 = (int((upper_bound - _offset) / _slope), upper_bound)
is_valid = True
for _x, _y in [new_p1, new_p2]:
if _x < 0:
logger.warning(
'line extends too far left, throwing out %s',
(_x, _y))
is_valid = False
elif _x > horizontal_limit:
logger.warning(
'line extends too far right, throwing out %s',
(_x, _y))
is_valid = False
if is_valid:
interpolations[_line] = {
'slope': _slope,
'offset': _offset,
'p1': new_p1,
'p2': new_p2
}
except Exception as err:
logger.error('interpolation error: %s', err)
def draw_lines(self,
lines: ndarray,
image=None,
color=None,
thickness=2) -> ndarray:
'''
Lines are drawn on the image inplace.
"""
:param image: image to apply lines
:param lines: numpy.ndarray of tuple (x1,y1,x2,y2)
:param color: tuple (r, g, b) uint8
:param thickness: pixel width of highlight
:return <numpy.ndarray>
'''
if image is not None:
assert image.shape == self.image.shape, 'images must be same shape, to draw lines'
self.image_tf = image
y_height, x_width, channels = self.image.shape
self.image_tf = zeros((y_height, x_width, channels), dtype=uint8)
# assign default color
if color is None:
color = [255, 0, 0]
# draw lines
logger.debug(
'-------------------------------------------------------------')
# use accumulated signals
region_mask = array(self.get_roi_mask(), dtype=bool)
upper_bound = int(y_height - 1)
lower_bound = int(upper_bound / 2 + self.Y_OFFSET)
horizontal_limit = (x_width - 1)
# quantify signals
mean_slope = find_dominate_signals(lines, self.roi_filter_lines,
region_mask)
mean_slope = find_mean_slope(self.roi_filter_lines, mean_slope)
# extend lines into lanes
interpolate_dominate_lines(self.roi_filter_lines,
self.slope_filter_lines, mean_slope,
lower_bound, upper_bound, horizontal_limit)
self.right_lane, self.left_lane = convert_lane_edges_to_polygons(
self.slope_filter_lines, lower_bound, upper_bound)
# fill lane polygons on images
if self.right_lane.any():
fillPoly(self.image_tf, int32([self.right_lane]), color)
else:
logger.error('did not create right-side lane')
if self.left_lane.any():
fillPoly(self.image_tf, int32([self.left_lane]), color)
else:
logger.error('did not create left-side lane')
return self.image_tf
def convert_lane_edges_to_polygons(
edges: dict,
lower_bound: int,
upper_bound: int,
lower_x_offset=LOWER_X_OFFSET,
upper_x_offset=UPPER_X_OFFSET) -> (array, array):
'''
Converts interpolated lines lanes into lane polygons
:param edges: dict of line info
:param lower_bound: lower y value in image
:param upper_bound: upper y value in image
:return: <tuple> (left_lane: array, right_lane: array)
'''
left = {}
right = {}
for _line in edges:
_slope = edges[_line]['slope']
if _slope < 0:
right[_line] = edges[_line]
else:
left[_line] = edges[_line]
if left:
mean_left_lower, mean_left_upper = get_point_stats(left, lower_bound)
poly_left = array([((mean_left_lower - lower_x_offset), lower_bound),
((mean_left_upper - upper_x_offset), upper_bound),
((mean_left_upper + upper_x_offset), upper_bound),
((mean_left_lower + lower_x_offset), lower_bound)])
else:
poly_left = array(None)
if right:
mean_right_lower, mean_right_upper = get_point_stats(
right, lower_bound)
poly_right = array([((mean_right_lower - lower_x_offset), lower_bound),
((mean_right_upper - upper_x_offset), upper_bound),
((mean_right_upper + upper_x_offset), upper_bound),
((mean_right_lower + lower_x_offset), lower_bound)
])
else:
poly_right = array(None)
logger.debug('polygons (left-lane, right-lane):\n(array(%s),\narray(%s))',
poly_left, poly_right)
return poly_left, poly_right
def find_dominate_signals(lines: ndarray,
signals: dict,
region_mask: array,
slope_max_cutoff=SLOPE_MAX_CUTOFF,
slope_thresh=SLOPE_THRESHOLD,
magnitude_thresh=MAGNITUDE_THRESH) -> float:
'''
Filters subset of dominate signals in line segments and returns mean slope
:param lines: <numpy.ndarray> line segments
:param signals dict filtered by one point valid in ROI
:param region_mask: ROI mask shape
:param slope_thresh: filters by slope general lane pitch
:param magnitude_thresh: filters lines by dominant signal length
:return: mean_slope: int of dominant signals
'''
i = 0
max_slope = 0.0
min_slope = 0.0
max_signal = 0
x_valid = 0
try:
for _line in lines:
for x1, y1, x2, y2 in _line:
if x1 == x2 or y1 == y2:
logger.warning('disregarding: %s, %s', (x1, y1), (x2, y2))
continue
if not valid_within_fov([(x1, y1), (x2, y2)], region_mask):
logger.debug('invalid in FOV, thowing out %s',
(x1, y1, x2, y2))
continue
slope = (y2 - y1) / (x2 - x1)
assert slope != 0, 'slope is zero {0}'.format(_line)
offset = y1 - (x1 * slope)
magnitude = sqrt((square(x2 - x1) + square(y2 - y1)))
abs_slope = abs(slope)
if magnitude > magnitude_thresh and abs_slope > slope_thresh and abs_slope < slope_max_cutoff:
if min_slope == 0:
min_slope = abs_slope
else:
min_slope = min(abs_slope, min_slope)
logger.debug(
'points: %s\t slope: %6.3f\t magnitude: %6.3f',
(x1, y1, x2, y2), slope, magnitude)
if magnitude > max_signal:
max_slope = abs(slope)
max_signal = magnitude
signals[i] = {
'slope': slope,
'offset': offset,
'magnitude': magnitude,
'p1': (x1, y1),
'p2': (x2, y2)
}
i = i + 1
except Exception as err:
logger.error('bad signal: %s in %s', err, signals)
mean_slope = (max_slope + min_slope) / 2
logger.debug('mean_slope: %s, max: %s, min: %s, max_signal: %s',
mean_slope, max_slope, min_slope, max_signal)
return mean_slope
def get_slope_stats(slopes: list, threshold=SLOPE_THRESHOLD) -> dict:
'''
Gives statistics on slope variances
:param slopes: list of all slopes
:return: <list> [lane_label, min, max, mean, std]
'''
_ret = {}
try:
for _slopes in slopes:
_min = -threshold
_max = threshold
_lane = '?'
_stdev = []
if _slopes:
for _slope in _slopes:
if not _slope:
continue
elif _slope < 0:
_lane = 'right'
else:
_lane = 'left'
_min = min(_slope, _min)
_max = max(_slope, _max)
assert len(_slopes) != 0, 'no slopes found'
_mean = sum(_slopes) / len(_slopes)
for _slope in _slopes:
_stdev.append(square(_slope - _mean))
_std = sum(_stdev) / len(_slopes)
_ret[_lane] = {
'min': _min,
'max': _max,
'mean': _mean,
'std': _std
}
else:
logger.debug('no slopes in input')
except Exception as err:
logger.error('bad stats: %s in %s', err, slopes)
return _ret