class RingBufferTests(unittest.TestCase):
def setUp(self):
self.buffer = RingBuffer(5)
def test_ring_buffer(self):
self.assertEqual(len(self.buffer.storage), 5)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
print('storage:', self.buffer.storage)
self.assertEqual(len(self.buffer.storage), 5)
print('buffer get', self.buffer.get())
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])
self.buffer.append('e')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])
self.buffer.append('f')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])
self.buffer.append('g')
self.buffer.append('h')
self.buffer.append('i')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.capacity = 5
self.buffer = RingBuffer(self.capacity)
def test_new_buffer_has_appropriate_capacity(self):
self.assertEqual(self.buffer.capacity, self.capacity)
def test_adding_one_element_to_buffer(self):
self.buffer.append('a')
self.assertEqual(self.buffer.get(), ['a'])
def test_filling_buffer_to_capacity(self):
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.buffer.append('e')
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])
def test_adding_one_element_to_full_buffer(self):
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.buffer.append('e')
self.buffer.append('f')
self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.buffer = RingBuffer(5)
self.buffer_2 = RingBuffer(5)
def test_ring_buffer(self):
self.assertEqual(self.buffer.storage.length, 0)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.assertEqual(self.buffer.storage.length, 4)
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])
self.buffer.append('e')
self.assertEqual(self.buffer.storage.length, 5)
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])
self.buffer.append('f')
self.assertEqual(self.buffer.storage.length, 5)
self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])
self.buffer.append('g')
self.buffer.append('h')
self.buffer.append('i')
self.assertEqual(self.buffer.storage.length, 5)
self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])
self.buffer.append('j')
self.buffer.append('k')
self.assertEqual(self.buffer.get(), ['k', 'g', 'h', 'i', 'j'])
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.capacity = 5
self.buffer = RingBuffer(self.capacity)
def test_new_buffer_has_appropriate_capacity(self):
print("\n", 1)
self.assertEqual(self.buffer.capacity, self.capacity)
def test_adding_one_element_to_buffer(self):
print("\n", 2)
self.buffer.append('a')
self.assertEqual(self.buffer.get(), ['a'])
def test_filling_buffer_to_capacity(self):
print("\n", 3)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.buffer.append('e')
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])
def test_adding_one_element_to_full_buffer(self):
print("\n", 4)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.buffer.append('e')
self.buffer.append('f')
self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])
def test_adding_many_elements_to_full_buffer(self):
print("\n", 5)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.buffer.append('e')
self.buffer.append('f')
self.buffer.append('g')
self.buffer.append('h')
self.buffer.append('i')
self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])
def test_adding_50_elements_to_buffer(self):
print("\n", 6)
for i in range(50):
self.buffer.append(i)
self.assertEqual(self.buffer.get(), [45, 46, 47, 48, 49])
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.capacity = 5
self.buffer = RingBuffer(self.capacity)
def test_new_buffer_has_appropriate_capacity(self):
self.assertEqual(self.buffer.capacity, self.capacity)
def test_adding_one_element_to_buffer(self):
self.buffer.append("a")
self.assertEqual(self.buffer.get(), ["a"])
def test_filling_buffer_to_capacity(self):
self.buffer.append("a")
self.buffer.append("b")
self.buffer.append("c")
self.buffer.append("d")
self.buffer.append("e")
self.assertEqual(self.buffer.get(), ["a", "b", "c", "d", "e"])
def test_adding_one_element_to_full_buffer(self):
self.buffer.append("a")
self.buffer.append("b")
self.buffer.append("c")
self.buffer.append("d")
self.buffer.append("e")
self.buffer.append("f")
self.assertEqual(self.buffer.get(), ["f", "b", "c", "d", "e"])
def test_adding_many_elements_to_full_buffer(self):
self.buffer.append("a")
self.buffer.append("b")
self.buffer.append("c")
self.buffer.append("d")
self.buffer.append("e")
self.buffer.append("f")
self.buffer.append("g")
self.buffer.append("h")
self.buffer.append("i")
self.assertEqual(self.buffer.get(), ["f", "g", "h", "i", "e"])
def test_adding_50_elements_to_buffer(self):
for i in range(50):
self.buffer.append(i)
self.assertEqual(self.buffer.get(), [45, 46, 47, 48, 49])
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.buffer = RingBuffer(5)
self.buffer_2 = RingBuffer(5)
def test_ring_buffer(self):
self.assertEqual(len(self.buffer.storage), 5)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.buffer = RingBuffer(5)
self.buffer_2 = RingBuffer(5)
def test_ring_buffer(self):
self.assertEqual(len(self.buffer.storage), 5)
self.buffer.append('a')
self.buffer.append('b')
self.buffer.append('c')
self.buffer.append('d')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])
self.buffer.append('e')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])
self.buffer.append('f')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])
self.buffer.append('g')
self.buffer.append('h')
self.buffer.append('i')
self.assertEqual(len(self.buffer.storage), 5)
self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])
self.buffer.append('j')
self.buffer.append('k')
self.assertEqual(self.buffer.get(), ['k', 'g', 'h', 'i', 'j'])
for i in range(50):
self.buffer_2.append(i)
self.assertEqual(self.buffer2.get(), [45, 46, 47, 48, 49])
class DetectGoal:
_HISTORY_LENGTH = 5
def __init__(self, options):
self.options = options["Goals"]
self.ball_in_field_history = RingBuffer(self.options['HistoryLength'])
self.ball_in_left_goal_history = RingBuffer(2 * self.options['HistoryLength'])
self.ball_in_right_goal_history = RingBuffer(2 * self.options['HistoryLength'])
def detect(self, image, ball):
goal_keep_mask = np.zeros([image.shape[0], image.shape[1]], np.uint8)
height, width, channels = image.shape
# left goal keep
left = self.options['Gates'][0]
cv2.rectangle(goal_keep_mask, tuple(left[0]), tuple(left[1]), (255, 255, 255), thickness=-1)
# right goal keep
right = self.options['Gates'][1]
cv2.rectangle(goal_keep_mask, tuple(right[0]), tuple(right[1]), (255, 255, 255), thickness=-1)
ball_boundaries = ball.get_boundaries(width)
self.ball_in_field_history.extend(np.array(ball.is_visible()))
self.ball_in_left_goal_history.extend(goal_keep_mask[ball_boundaries[0][1], ball_boundaries[0][0]] == 255)
self.ball_in_right_goal_history.extend(goal_keep_mask[ball_boundaries[1][1], ball_boundaries[1][0]] == 255)
if self._detect_goal(self.ball_in_left_goal_history):
return [False, True]
elif self._detect_goal(self.ball_in_right_goal_history):
return [True, False]
else:
return [False, False]
def _detect_goal(self, ball_in_goal_area_history):
"""
Checks goal within goal area and within history of frames
:param ball_in_goal_area_history:
:return: whether goal was detected
"""
if np.any(ball_in_goal_area_history.get()) and not np.any(self.ball_in_field_history.get()):
ball_in_goal_area_history.clear()
return True
return False
class RingBufferTests(unittest.TestCase):
def setUp(self):
self.buffer = RingBuffer(5)
self.buffer_2 = RingBuffer(5)
def test_ring_buffer(self):
self.assertEqual(self.buffer.storage.length, 0)
self.buffer.append("a")
self.buffer.append("b")
self.buffer.append("c")
self.buffer.append("d")
self.assertEqual(self.buffer.storage.length, 4)
self.assertEqual(self.buffer.get(), ["a", "b", "c", "d"])
self.buffer.append("e")
self.assertEqual(self.buffer.storage.length, 5)
self.assertEqual(self.buffer.get(), ["a", "b", "c", "d", "e"])
self.buffer.append("f")
self.assertEqual(self.buffer.storage.length, 5)
self.assertEqual(self.buffer.get(), ["f", "b", "c", "d", "e"])
self.buffer.append("g")
self.buffer.append("h")
self.buffer.append("i")
self.assertEqual(self.buffer.storage.length, 5)
self.assertEqual(self.buffer.get(), ["f", "g", "h", "i", "e"])
self.buffer.append("j")
self.buffer.append("k")
self.assertEqual(self.buffer.get(), ["k", "g", "h", "i", "j"])
for i in range(50):
self.buffer_2.append(i)
self.assertEqual(self.buffer_2.get(), [45, 46, 47, 48, 49])
class SoundEffectEngine(EffectEngine):
def __init__(self, queue):
super().__init__(queue)
self._name = "SoundEffectEngine"
self._chunk = 1024
self._player = pyaudio.PyAudio()
self._wav_dir = config.SOUNDFILE_DIR
self._wav_files = {}
self._cur_wav_file = None
self._stream = None
self._dataFile = open("history.csv", "w")
self._dataWriter = csv.writer(self._dataFile,
delimiter=",",
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
self._currentBpms = RingBuffer(config.AVG_WINDOW)
self._heartbeat = config.HEARTBEAT_TIMEOUT
if config.BPM_RANGE_LOW >= config.BPM_RANGE_HIGH:
raise ValueError(
"BPM Ranges are not configured correctly in config")
def read_wav_files(self):
self._wav_files = [
join(self._wav_dir, f) for f in listdir(self._wav_dir)
if isfile(join(self._wav_dir, f)) and ".wav" in f
]
self._wav_files = natsorted(self._wav_files, key=lambda y: y.lower())
if len(self._wav_files) < 1:
raise FileNotFoundError("No wav files found in given directory.")
def run(self):
print("Starting " + self._name)
self.read_wav_files()
self.effect_loop()
def shutdown_audio(self):
self._cur_wav_file.close()
self._stream.stop_stream()
try:
self._player.close(self._stream)
except ValueError:
pass
def idle(self):
print(self._name + " idling..")
self.shutdown_audio()
self._currentBpms.reset()
while not self._stop_event.is_set():
try:
bpm = self._queue.get(timeout=2)
if bpm > 0:
self._currentBpms.append(bpm)
self.open_wav_file()
self._heartbeat = config.HEARTBEAT_TIMEOUT
break
except queue.Empty:
pass
def poll_bpm(self):
try:
bpm = self._queue.get_nowait()
self._heartbeat = config.HEARTBEAT_TIMEOUT
if bpm > 0:
self._currentBpms.append(bpm)
self._bpmHistory.append(bpm)
# write into csv
self._dataWriter.writerow([bpm])
self._dataFile.flush()
# save data history
except queue.Empty:
self._heartbeat = self._heartbeat - 1
def open_wav_file(self):
index = self.choose_wav_file()
self._cur_wav_file = wave.open(self._wav_files[index], "rb")
self._stream = self._player.open(
format=self._player.get_format_from_width(
self._cur_wav_file.getsampwidth()),
channels=self._cur_wav_file.getnchannels(),
rate=self._cur_wav_file.getframerate(),
output=True)
def choose_wav_file(self):
if self._currentBpms.get_len() is 0:
return 0
if config.SOUND_MODE is config.SoundMode.AVERAGE:
bpm = self._currentBpms.get_sum() / self._currentBpms.get_len()
limited_bpm = config.BPM_RANGE_LOW if bpm < config.BPM_RANGE_LOW else \
config.BPM_RANGE_HIGH if bpm > config.BPM_RANGE_HIGH else bpm
total_range = config.BPM_RANGE_HIGH - config.BPM_RANGE_LOW
index = math.floor((limited_bpm - config.BPM_RANGE_LOW) /
math.ceil(total_range / len(self._wav_files)))
elif config.SOUND_MODE is config.SoundMode.DIFFERENCE:
diff = max(self._currentBpms.get()) - min(self._currentBpms.get())
if diff > len(self._wav_files) - 1:
diff = len(self._wav_files) - 1
index = diff
print(index)
return index
def effect_loop(self):
self.poll_bpm()
self.open_wav_file()
try:
while True:
if self._stop_event.is_set():
self.shutdown_audio()
return
data = self._cur_wav_file.readframes(self._chunk)
while data != b'':
self._stream.write(data)
data = self._cur_wav_file.readframes(self._chunk)
self.poll_bpm()
if self._heartbeat is 0:
self.idle()
else:
self.shutdown_audio()
self.open_wav_file()
time.sleep(0.1)
except KeyboardInterrupt:
print("Got interrupt, crunching Data...")
def main():
model = load_model('./tmp/1521265089/model-99.h5')
_, _, _, int_to_label = files_and_labels('./data')
ring_buffer = RingBuffer(SAMPLE_RATE)
audio = pyaudio.PyAudio()
stream = audio.open(format=FORMAT,
channels=CHANNELS,
rate=SAMPLE_RATE,
input=True,
frames_per_buffer=CHUNK)
output = audio.open(format=FORMAT,
channels=CHANNELS,
rate=SAMPLE_RATE,
frames_per_buffer=CHUNK,
output=True)
stream.start_stream()
output.start_stream()
silence = chr(0) * CHUNK * CHANNELS * 2
audio_data = tf.placeholder(tf.float32, [SAMPLE_RATE, 1])
mfcc = encode_data(audio_data, SAMPLE_RATE)
while True:
available = stream.get_read_available()
if available > 0:
for _ in range(int(available / CHUNK)):
data = stream.read(CHUNK)
ring_buffer.append(data)
# output.write(data)
else:
output.write(silence)
raw_audio_data = np.array(ring_buffer.get())
to_audio_data = raw_audio_data.reshape([SAMPLE_RATE, 1])
feed_dict = {
audio_data: to_audio_data
}
with tf.Session() as sess:
mfcc_result = sess.run(mfcc, feed_dict)
x = mfcc_result.reshape(-1, 98, 40, 1)
predictions = model.predict(x)
classes = np.argmax(predictions, axis=1)
for prediction in classes:
label = int_to_label[prediction]
print(label)
stream.stop_stream()
stream.close()
audio.terminate()
class LaneFitter(object):
# Define conversions in x and y from pixels space to meters
ym_per_pix = 30 / 720 # meters per pixel in y dimension
xm_per_pix = 3.7 / 700 # meters per pixel in x dimension
def __init__(self, p_matrix, verbose=False):
self._left = Lane(None, None, None, None)
self._right: Lane(None, None, None, None)
self._vebose = verbose
self._M, self._M_inv = p_matrix
self._buffer = RingBuffer(5)
def _compute_average_base(self, bases):
items = self._buffer.get()
if len(items) > 0:
left_base = list(map(lambda x: x[0].base, items))
right_base = list(map(lambda x: x[1].base, items))
if bases is not None:
left_base.append(bases[0])
right_base.append(bases[1])
w = np.logspace(0, 1, len(left_base))
w /= sum(w)
left_base = int(np.average(left_base, weights=w))
right_base = int(np.average(right_base, weights=w))
return left_base, right_base
return bases
def _compute_average_fit(self, new_fit):
items = self._buffer.get()
left_fit = list(map(lambda x: x[0].fit_params, items))
right_fit = list(map(lambda x: x[1].fit_params, items))
if new_fit is not None and new_fit[0] is not None and new_fit[
1] is not None:
left_fit.append(new_fit[0])
right_fit.append(new_fit[1])
w = np.logspace(0, 1, len(left_fit))
w /= sum(w)
left_fit = np.average(np.array(left_fit), axis=0, weights=w)
right_fit = np.average(np.array(right_fit), axis=0, weights=w)
return left_fit, right_fit
def _compute_curvature(self, ploty, fit, fitx) -> Curvature:
# compute curvature
# Define y-value where we want radius of curvature
y_eval = np.max(ploty)
curve_rad = (
(1 +
(2 * fit[0] * y_eval + fit[1])**2)**1.5) / np.absolute(2 * fit[0])
fit_cr = np.polyfit(ploty * LaneFitter.ym_per_pix,
fitx * LaneFitter.xm_per_pix, 2)
# Calculate the new radii of curvature
curve_rad_world = (
(1 +
(2 * fit_cr[0] * y_eval * LaneFitter.ym_per_pix + fit_cr[1])**2)**
1.5) / np.absolute(2 * fit_cr[0])
return Curvature(curve_rad, curve_rad_world)
def _compute_slope_mse(self, ploty, fit):
left_fit, right_fit = fit
left_slope = 2 * ploty * left_fit[0] + left_fit[1]
right_slope = 2 * ploty * right_fit[0] + right_fit[1]
mse = np.sum(np.power(left_slope - right_slope, 2)) / len(left_slope)
return mse
def fit_polynomial(self, y, x):
if len(x) > 0 and len(y) > 0:
return np.polyfit(y, x, 2)
return None
def draw_line(self, img, y, x):
points = np.zeros((len(y), 2), dtype=np.int32)
points[:, 1] = y.astype(np.int32)
points[:, 0] = x.astype(np.int32)
points = points.reshape((-1, 1, 2))
cv2.polylines(img, points, True, (0, 255, 255), thickness=2)
def fit(self, image):
"""
This method find the line pixels on the bird eye view binary image
:param image:
:return:
"""
imshape = image.shape
out_img = np.dstack([image, image, image]) * 255
n_windows = 9
window_height = imshape[0] // n_windows
margin = 100
minpix = 50
hist = np.sum(image[imshape[0] // 2:, :], axis=0)
midpoint = hist.shape[0] // 2
current_frame_left_base = np.argmax(hist[:midpoint])
current_frame_right_base = np.argmax(hist[midpoint:]) + midpoint
if self._left.fit_params is None or self._right.fit_params is None:
left_base, right_base = current_frame_left_base, current_frame_right_base
nonzero = image.nonzero()
nonzero_y = np.array(nonzero[0])
nonzero_x = np.array(nonzero[1])
left_lane_indices, right_lane_indices = [], []
leftx_current = left_base
rightx_current = right_base
for window in range(n_windows):
window_y_low = imshape[0] - (window + 1) * window_height
window_y_high = imshape[0] - window * window_height
window_x_left_low = leftx_current - margin
window_x_left_high = leftx_current + margin
window_x_right_low = rightx_current - margin
window_x_right_high = rightx_current + margin
left_indices = ((nonzero_y >= window_y_low) &
(nonzero_y < window_y_high) &
(nonzero_x >= window_x_left_low) &
(nonzero_x <= window_x_left_high)).nonzero()[0]
right_indices = (
(nonzero_y >= window_y_low) & (nonzero_y < window_y_high) &
(nonzero_x >= window_x_right_low) &
(nonzero_x <= window_x_right_high)).nonzero()[0]
left_lane_indices.append(left_indices)
right_lane_indices.append(right_indices)
if len(left_indices) > minpix:
leftx_current = np.int(np.mean(nonzero_x[left_indices]))
if len(right_indices) > minpix:
rightx_current = np.int(np.mean(nonzero_x[right_indices]))
left_lane_indices = np.concatenate(left_lane_indices)
right_lane_indices = np.concatenate(right_lane_indices)
leftx = nonzero_x[left_lane_indices]
lefty = nonzero_y[left_lane_indices]
rightx = nonzero_x[right_lane_indices]
righty = nonzero_y[right_lane_indices]
else:
left_base, right_base = self._compute_average_base(None)
nonzero = image.nonzero()
nonzero_y = nonzero[0]
nonzero_x = nonzero[1]
left_fit = self._left.fit_params
right_fit = self._right.fit_params
margin = 100
left_lane_indices = ((nonzero_x > (left_fit[0] * (nonzero_y ** 2) + left_fit[1] * nonzero_y + left_fit[2] - margin)) & \
(nonzero_x < (left_fit[0] * (nonzero_y ** 2) + left_fit[1] * nonzero_y + left_fit[2] + margin)))
right_lane_indices = ((nonzero_x >
(right_fit[0] *
(nonzero_y**2) + right_fit[1] * nonzero_y +
right_fit[2] - margin)) &
(nonzero_x <
(right_fit[0] *
(nonzero_y**2) + right_fit[1] * nonzero_y +
right_fit[2] + margin)))
leftx = nonzero_x[left_lane_indices]
lefty = nonzero_y[left_lane_indices]
rightx = nonzero_x[right_lane_indices]
righty = nonzero_y[right_lane_indices]
left_fit = self.fit_polynomial(lefty, leftx)
right_fit = self.fit_polynomial(righty, rightx)
leftx_current = left_base
rightx_current = right_base
for window in range(n_windows):
window_y_low = imshape[0] - (window + 1) * window_height
window_y_high = imshape[0] - window * window_height
window_x_left_low = leftx_current - margin
window_x_left_high = leftx_current + margin
window_x_right_low = rightx_current - margin
window_x_right_high = rightx_current + margin
cv2.rectangle(out_img, (window_x_left_low, window_y_low),
(window_x_left_high, window_y_high), (0, 255, 0), 2)
cv2.rectangle(out_img, (window_x_right_low, window_y_low),
(window_x_right_high, window_y_high), (0, 255, 0), 2)
ploty = np.linspace(0, image.shape[0] - 1, image.shape[0])
# do sanity checking, are two lines relatively parallel?
if left_fit is not None and right_fit is not None:
slope_diff = self._compute_slope_mse(ploty, (left_fit, right_fit))
if slope_diff > 0.1:
print('ignoring fit')
# ignore this fit
left_fit, right_fit = None, None
left_fit, right_fit = self._compute_average_fit((left_fit, right_fit))
# Generate x and y values for plotting
left_fitx = left_fit[0] * ploty**2 + left_fit[1] * ploty + left_fit[2]
right_fitx = right_fit[0] * ploty**2 + right_fit[
1] * ploty + right_fit[2]
out_img[nonzero_y[left_lane_indices],
nonzero_x[left_lane_indices]] = [255, 0, 0]
out_img[nonzero_y[right_lane_indices],
nonzero_x[right_lane_indices]] = [0, 0, 255]
self.draw_line(out_img, ploty, left_fitx)
self.draw_line(out_img, ploty, right_fitx)
left_curvature = self._compute_curvature(ploty, left_fit, left_fitx)
right_curvature = self._compute_curvature(ploty, right_fit, right_fitx)
offset = LaneFitter.xm_per_pix * (
(current_frame_left_base +
(current_frame_right_base - current_frame_left_base) / 2) -
imshape[1] // 2)
self._left = Lane(left_base, left_fit, left_fitx, left_curvature)
self._right = Lane(right_base, right_fit, right_fitx, right_curvature)
self._buffer.append((self._left, self._right))
logging.info('Curvature (pixel space): {}'.format(
(self._left.curvature.pixel, self._right.curvature.pixel)))
logging.info('Curvature (world space): {}'.format(
(self._left.curvature.world, self._right.curvature.world)))
logging.info('Distance from center: {:.2}m'.format(offset))
return out_img, offset
def transform(self, image):
"""
This method transforms the identified lanes back to the original image and draws the lanes on the road
:param image:
:return:
"""
# Create an image to draw the lines on
imshape = image.shape
color_warp = np.zeros(imshape, dtype=np.uint8)
left: Lane = self._left
right: Lane = self._right
ploty = np.linspace(0, image.shape[0] - 1, image.shape[0])
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left.fit_x, ploty]))])
pts_right = np.array(
[np.flipud(np.transpose(np.vstack([right.fit_x, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
# Warp the blank back to original image space using inverse perspective matrix (Minv)
new_warp = cv2.warpPerspective(color_warp, self._M_inv,
(imshape[1], imshape[0]))
# Combine the result with the original image
result = cv2.addWeighted(image, 1, new_warp, 0.3, 0)
return result, (left.curvature, right.curvature)
def fit_transform(self, binary_image, color_image):
fit_image, offset = self.fit(binary_image)
final_image, curvatures = self.transform(color_image)
return final_image, fit_image, curvatures, offset
from ring_buffer import RingBuffer
buffer = RingBuffer(3)
print(buffer.get()) # should return []
buffer.append('a')
buffer.append('b')
buffer.append('c')
print(buffer.get()) # should return ['a', 'b', 'c']
# 'd' overwrites the oldest value in the ring buffer, which is 'a'
buffer.append('d')
print(buffer.get()) # should return ['d', 'b', 'c']
buffer.append('e')
buffer.append('f')
print(buffer.get()) # should return ['d', 'e', 'f']
class LineTracer():
def __init__(self):
self.last5_left = RingBuffer(5)
self.last5_right = RingBuffer(5)
self.avg_left_fit = 0
self.avg_right_fit = 0
self.avg_left_curv = 0
self.avg_right_curv = 0
self.avg_left_dist = 0
self.avg_right_dist = 0
def getLast(self):
if len(self.last5_left.get()) > 0 and len(self.last5_right.get()) > 0:
return [self.last5_left.get()[-1], self.last5_right.get()[-1]]
else:
return generate_empty_lines()
def getAvg(self):
left_list = self.last5_left.get()
right_list = self.last5_right.get()
num_lines = len(left_list)
#print(num_lines)
ploty = self.last5_left.get()[-1].fity
sum_curvl = 0
sum_curvr = 0
sum_distl = 0
sum_distr = 0
sum_fitl = [0, 0, 0]
sum_fitr = [0, 0, 0]
if num_lines > 0:
for left in left_list:
sum_curvl += left.curvature
sum_distl += left.dist2line
sum_fitl = np.add(sum_fitl, left.fit)
for right in right_list:
sum_curvr += right.curvature
sum_distr += right.dist2line
sum_fitr = np.add(sum_fitr, right.fit)
curvl = sum_curvl / num_lines
curvr = sum_curvr / num_lines
distl = sum_distl / num_lines
distr = sum_distr / num_lines
left_fit = np.divide(sum_fitl, num_lines)
right_fit = np.divide(sum_fitr, num_lines)
#print(left_fit)
# Draw the fit line plain
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
left_fitx = left_fitx.astype(int)
right_fitx = right_fitx.astype(int)
leftl = SingleLine(True,left_fit, left_fitx,ploty,curvl,distl,\
self.last5_left.get()[-1].allx, self.last5_left.get()[-1].ally)
rightl = SingleLine(True,right_fit, right_fitx,ploty,curvr,distr,\
self.last5_right.get()[-1].allx, self.last5_right.get()[-1].ally)
return leftl, rightl
def newLine(self, new_left, new_right):
if new_left.detected == False or new_right.detected == False:
#self.last5_left.append([False])
#self.last5_right.append([False])
pass
else:
self.last5_left.append(new_left)
self.last5_right.append(new_right)
left_list = self.last5_left.get()
right_list = self.last5_right.get()
num_lines = len(left_list)
if num_lines > 0:
for left in left_list:
#self.avg_left_fit = np.add(self.avg_left_fit, left.fit)
self.avg_left_curv += left.curvature
self.avg_left_dist += left.dist2line
for right in right_list:
#self.avg_right_fit = np.add(self.avg_right_fit, right.fit)
self.avg_right_curv += right.curvature
self.avg_right_dist += right.dist2line
#self.avg_left_fit = np.divide(self.avg_left_fit, num_lines)
#self.avg_right_fit = np.divide(self.avg_right_fit, num_lines)
self.avg_left_curv /= num_lines
self.avg_right_curv /= num_lines
self.avg_left_dist /= num_lines
self.avg_right_dist /= num_lines
