def run_test(id_test, test, id_estimator, estimator):
global W, H
logger.info(' id_test: %s' % id_test)
logger.info('id_estimator: %s' % id_estimator)
from led_detection.unit_tests import LEDDetectionUnitTest
assert isinstance(test, LEDDetectionUnitTest)
query = test.get_query()
print( query['images']['rgb'][0].shape)
H, W, _ = query['images']['rgb'][0].shape
print('shape is %s, %s'%(W, H))
result = estimator.detect_led(**query)
# We are testing whether the expected detections are a subset of
# the returned ones, we will accept duplicate detections of the
# same LED
match_count = [0]*len(test.expected)
for r in result.detections:
m = find_match(r, test.expected)
if(m != -1):
match_count[m]+=1
missedLEDs = [test.expected[i] for i in range(0, len(match_count)) if match_count[i]==0]
if(missedLEDs):
logger.error('missed LED detections (%s): \n %s' % (len(missedLEDs),missedLEDs))
# Valerio: check this - sometimes the error above is thrown, but the test
# does not return False
return not 0 in match_count
def run_test(id_test, test, id_estimator, estimator):
global W, H
logger.info(' id_test: %s' % id_test)
logger.info('id_estimator: %s' % id_estimator)
from led_detection.unit_tests import LEDDetectionUnitTest
assert isinstance(test, LEDDetectionUnitTest)
query = test.get_query()
print( query['images']['rgb'][0].shape)
H, W, _ = query['images']['rgb'][0].shape
print('shape is %s, %s'%(W, H))
result = estimator.detect_led(**query)
# We are testing whether the expected detections are a subset of
# the returned ones, we will accept duplicate detections of the
# same LED
match_count = [0]*len(test.expected)
for r in result.detections:
m = find_match(r, test.expected)
if(m != -1):
match_count[m]+=1
missedLEDs = [test.expected[i] for i in range(0, len(match_count)) if match_count[i]==0]
if(missedLEDs):
logger.error('missed LED detections (%s): \n %s' % (len(missedLEDs),missedLEDs))
return not 0 in match_count
def run_test(id_test, test, id_estimator, estimator):
logger.info(' id_test: %s' % id_test)
logger.info('id_estimator: %s' % id_estimator)
from led_detection.unit_tests import LEDDetectionUnitTest
assert isinstance(test, LEDDetectionUnitTest)
query = test.get_query()
result = estimator.detect_led(**query)
raise NotImplementedError('To finish: Compare result to expected result.')
def run_test(id_test, test, id_estimator, estimator):
logger.info(' id_test: %s' % id_test)
logger.info('id_estimator: %s' % id_estimator)
from led_detection.unit_tests import LEDDetectionUnitTest
assert isinstance(test, LEDDetectionUnitTest)
query = test.get_query()
result = estimator.detect_led(**query)
raise NotImplementedError('To finish: Compare result to expected result.')
def main():
script_name = os.path.basename(sys.argv[0])
args = sys.argv[1:]
if len(args) != 2:
msg = """
Usage:
rosrun led_detection <script> <tests> <algorithms>
where:
<tests> = comma separated list of algorithms. May use "*".
<algorithms> = comma separated list of algorithms. May use "*".
For example, this runs all tests on all algorithms:
rosrun led_detection <script> '*' '*'
"""
msg = msg.replace('<script>', script_name)
logger.error(msg)
sys.exit(2)
which_tests0 = sys.argv[1]
which_estimators0 = sys.argv[2]
root = os.environ['DUCKIETOWN_ROOT']
dirname = 'catkin_ws/src/led_detection/scripts/'
filename = '20160312-allblinking_test1-argo.led_detection_test.yaml'
filename = os.path.join(root, dirname, filename)
alltests = load_tests(filename)
estimators = {'dummy': DummyLEDDetector()}
which_tests = expand_string(which_tests0, list(alltests))
which_estimators = expand_string(which_estimators0, list(estimators))
logger.info(' tests: %r |-> %s' % (which_tests0, which_tests))
logger.info('estimators: %r |-> %s' %
(which_estimators0, which_estimators))
# which tests to execute
for id_test in which_tests:
for id_estimator in which_estimators:
test_results = run_test(id_test, alltests[id_test], id_estimator,
estimators[id_estimator])
def main():
script_name = os.path.basename(sys.argv[0])
args = sys.argv[1:]
if len(args) != 2:
msg = """
Usage:
rosrun led_detection <script> <tests> <algorithms>
where:
<tests> = comma separated list of algorithms. May use "*".
<algorithms> = comma separated list of algorithms. May use "*".
For example, this runs all tests on all algorithms:
rosrun led_detection <script> '*' '*'
"""
msg = msg.replace('<script>', script_name)
logger.error(msg)
sys.exit(2)
which_tests0 = sys.argv[1]
which_estimators0 = sys.argv[2]
root = os.environ['DUCKIETOWN_ROOT']
dirname = 'catkin_ws/src/led_detection/scripts/'
filename = '20160312-allblinking_test1-argo.led_detection_test.yaml'
filename = os.path.join(root, dirname, filename)
alltests = load_tests(filename)
estimators = {'dummy': DummyLEDDetector()}
which_tests = expand_string(which_tests0, list(alltests))
which_estimators = expand_string(which_estimators0, list(estimators))
logger.info(' tests: %r |-> %s' % (which_tests0, which_tests))
logger.info('estimators: %r |-> %s' % (which_estimators0, which_estimators))
# which tests to execute
for id_test in which_tests:
for id_estimator in which_estimators:
test_results = run_test(id_test, alltests[id_test],
id_estimator, estimators[id_estimator])
def main():
script_name = os.path.basename(sys.argv[0])
args = sys.argv[1:]
if len(args) != 2:
msg = 'I need two arguments. Please see README.md for documentation.'
logger.error(msg)
sys.exit(2)
which_tests0 = sys.argv[1]
which_estimators0 = sys.argv[2]
package_dir = get_ros_package_path('led_detection')
logger.debug('Package dir: %r' % package_dir)
# dirname = 'catkin_ws/src/f23-LED/led_detection/scripts/'
#filename = 'all_tests.yaml'
filename = os.path.join(package_dir, 'scripts', 'dp45_tests.yaml')
alltests = load_tests(filename)
estimators = {
'baseline':
LEDDetector(ploteverything=False, verbose=True, plotfinal=False),
'LEDDetector_plots':
LEDDetector(True, True, True)
}
#,'LEDDetector_forloops' : LEDDetector_forloops(True, True, True)}
which_tests = expand_string(which_tests0, list(alltests))
which_estimators = expand_string(which_estimators0, list(estimators))
logger.info(' tests: %r |-> %s' % (which_tests0, which_tests))
logger.info('estimators: %r |-> %s' %
(which_estimators0, which_estimators))
# which tests to execute
test_results = {}
for id_test in which_tests:
for id_estimator in which_estimators:
result = run_test(id_test, alltests[id_test], id_estimator,
estimators[id_estimator])
test_results[(id_test, id_estimator)] = result
nfailed = list(test_results.values()).count(False)
if not nfailed:
logger.info('All tests passed')
else:
which = [k for k, v in test_results.items() if not v]
logger.error('These tests failed: %s ' % which)
sys.exit(3)
def main():
script_name = os.path.basename(sys.argv[0])
args = sys.argv[1:]
if len(args) != 2:
msg = """
Usage:
rosrun led_detection <script> <tests> <algorithms>
where:
<tests> = comma separated list of algorithms. May use "*".
<algorithms> = comma separated list of algorithms. May use "*".
For example, this runs all tests on all algorithms:
rosrun led_detection <script> '*' '*'
The default algorithm is called "baseline", and the tests are invoked using:
rosrun led_detection <script> '*' 'baseline'
"""
msg = msg.replace('<script>', script_name)
logger.error(msg)
sys.exit(2)
which_tests0 = sys.argv[1]
which_estimators0 = sys.argv[2]
root = os.environ['DUCKIETOWN_ROOT']
dirname = 'catkin_ws/src/f23-LED/led_detection/scripts/'
#filename = 'all_tests.yaml'
filename = 'dp45_tests.yaml'
filename = os.path.join(root, dirname, filename)
alltests = load_tests(filename)
estimators = {
'baseline':
LEDDetector(ploteverything=False, verbose=True, plotfinal=False),
'LEDDetector_plots':
LEDDetector(True, True, True)
}
#,'LEDDetector_forloops' : LEDDetector_forloops(True, True, True)}
which_tests = expand_string(which_tests0, list(alltests))
which_estimators = expand_string(which_estimators0, list(estimators))
logger.info(' tests: %r |-> %s' % (which_tests0, which_tests))
logger.info('estimators: %r |-> %s' %
(which_estimators0, which_estimators))
# which tests to execute
test_results = {}
for id_test in which_tests:
for id_estimator in which_estimators:
result = run_test(id_test, alltests[id_test], id_estimator,
estimators[id_estimator])
test_results[(id_test, id_estimator)] = result
nfailed = list(test_results.values()).count(False)
if not nfailed:
logger.info('All tests passed')
else:
which = [k for k, v in test_results.items() if not v]
logger.error('These tests failed: %s ' % which)
sys.exit(3)
def detect_led(self, images, mask, frequencies_to_detect,
min_distance_between_LEDs_pixels):
assert len(images.shape) == 1
n = images.shape[0]
if n == 0:
raise ValueError('No images provided')
timestamps = images['timestamp']
rgb = images['rgb']
rgb0 = rgb[0]
if not mask.shape == rgb0.shape:
raise ValueError('Invalid mask')
if not isinstance(frequencies_to_detect, list):
raise ValueError(frequencies_to_detect)
if not min_distance_between_LEDs_pixels > 0:
raise ValueError(min_distance_between_LEDs_pixels)
channel = images['rgb'][:, :, :, 0] # just using first channel
cell_width = 15
cell_height = 15
(cell_vals, crop_offset) = self.downsample(channel, cell_width,
cell_height)
candidates_mask = self.get_candidate_cells(cell_vals, 100)
candidate_cells = [(i, j)
for (i, j) in np.ndindex(candidates_mask.shape)
if candidates_mask[i, j]]
# Create result object
result = LEDDetectionArray()
# Detect frequencies and discard non-periodic signals
# ts_tolerance = 0.2 # unnecessary
f_tolerance = 0.25
min_num_periods = 3
for (i, j) in candidate_cells:
signal = cell_vals[:, i, j]
signal = signal - np.mean(signal)
zero_crossings = np.where(np.diff(np.sign(signal)))[0]
zero_crossings_t = timestamps[zero_crossings]
led_img_coords = Vector2D((0.5 + j) * cell_width + crop_offset[1],
(0.5 + i) * cell_height + crop_offset[0])
diffs = [
b - a for a, b in zip(zero_crossings_t, zero_crossings_t[1:])
]
if (self.verbose):
logger.info('Coords: %s, %s' %
(led_img_coords.x, led_img_coords.y))
logger.info('Zero crossings: %s' % zero_crossings_t)
logger.info('Diffs: %s' % diffs)
logger.info('Zero-crossing measured freq %s' %
(0.5 / np.mean(diffs)))
if (len(zero_crossings) < min_num_periods):
if (self.verbose):
logger.info('Not an LED, discarded\n')
continue
# Frequency estimation based on zero crossings - quite bad
#for f in frequencies_to_detect:
# if(all(d-ts_tolerance <= 0.5/f <= d+ts_tolerance for d in diffs)):
# if(self.verbose):
# logger.info('Confirmed LED with frequency %s\n'%f)
# recover coordinates of centroid
# result.detections.append(LEDDetection(timestamps[0], timestamps[-1],
# led_img_coords, f, '', -1)) # -1...confidence not implemented
# break
# Frequency estimation based on FFT
T = 1.0 / 30 # TODO expecting 30 fps, but RESAMPLE to be sure
f = np.linspace(0.0, 1.0 / (2.0 * T), n / 2)
signal_f = scipy.fftpack.fft(signal)
y_f = 2.0 / n * np.abs(signal_f[:n / 2])
fft_peak_freq = 1.0 * np.argmax(y_f) / T / n
if (self.verbose):
logger.info('FFT peak frequency: %s' % fft_peak_freq)
# Bin frequency into the ones to detect
freq = [
x for x in frequencies_to_detect
if abs(x - fft_peak_freq) < f_tolerance
]
if (freq):
result.detections.append(
LEDDetection(rospy.Time.from_sec(timestamps[0]),
rospy.Time.from_sec(timestamps[-1]),
led_img_coords, freq[0], '',
-1)) # -1...confidence not implemented
if (self.verbose):
logger.info('LED confirmed, frequency: %s' % freq)
else:
logger.info('Could not associate frequency, discarding')
print(signal.shape)
print(timestamps[:15].shape)
# Plot all signals and FFTs
if (self.ploteverything):
fig, ax1 = plt.subplots()
ax1.plot(timestamps[:15], signal)
fig, ax2 = plt.subplots()
ax2.plot(f[:15], y_f)
plt.show()
plt.imshow(rgb0)
ax = plt.gca()
font = {
'family': 'serif',
'color': 'red',
'weight': 'bold',
'size': 16,
}
# Plot all results
if (self.plotfinal):
for r in result.detections:
pos = r.pixels_normalized
ax.add_patch(
Rectangle(
(pos.x - 0.5 * cell_width, pos.y - 0.5 * cell_height),
cell_width,
cell_height,
edgecolor="red",
linewidth=3,
facecolor="none"))
plt.text(pos.x - 0.5 * cell_width,
pos.y - cell_height,
str(r.frequency),
fontdict=font)
plt.show()
return result
def detect_led(self,
images,
mask,
frequencies_to_detect,
min_distance_between_LEDs_pixels):
assert len(images.shape) == 1
n = images.shape[0]
if n == 0:
raise ValueError('No images provided')
timestamps = images['timestamp']
# print('timestamps: {0}'.format(timestamps))
rgb = images['rgb']
rgb0 = rgb[0]
if not mask.shape == rgb0.shape:
raise ValueError('Invalid mask')
if not isinstance(frequencies_to_detect, list):
raise ValueError(frequencies_to_detect)
if not min_distance_between_LEDs_pixels > 0:
raise ValueError(min_distance_between_LEDs_pixels)
#channel = images['rgb'][:,:,:,0] # just using first channel
# Go for the following lines if you want to use a grayscale image
# as an input instead of preferring one specific channel
channel = np.zeros(images['rgb'].shape[0:-1])
for i in range(n):
channel[i,:,:] = cv2.cvtColor(images['rgb'][i,:,:,:], cv2.COLOR_BGR2GRAY)
print('channel.shape {0}'.format(channel.shape))
W = channel.shape[2]
H = channel.shape[1]
cell_width = 15
cell_height = 15
var_threshold = 100
(cell_vals, crop_offset) = self.downsample(channel, cell_width, cell_height)
candidates_mask = self.get_candidate_cells(cell_vals, var_threshold)
candidate_cells = [(i,j) for (i,j) in np.ndindex(candidates_mask.shape) if candidates_mask[i,j]]
if(self.publisher is not None):
for idx in candidate_cells:
self.debug_msg.candidates.append(Vector2D(idx[0], idx[1]))
#self.republish()
# Create result object
result = LEDDetectionArray()
unfiltered = LEDDetectionArray()
# Detect frequencies and discard non-periodic signals
# ts_tolerance = 0.2 # unnecessary
f_tolerance = 0.3
for (i,j) in candidate_cells:
signal = cell_vals[:,i,j]
signal = signal-np.mean(signal)
led_img_coords = Vector2D((0.5+j)*cell_width+crop_offset[1], (0.5+i)*cell_height+crop_offset[0])
led_img_coords_norm = Vector2D(1.0*led_img_coords.x/W, 1.0*led_img_coords.y/H)
if(self.verbose):
logger.info('Coords: %s, %s'% (led_img_coords.x,led_img_coords.y))
# Frequency estimation based on FFT
T = 1.0/30 # TODO expecting 30 fps, but RESAMPLE to be sure
f = np.linspace(0.0, 1.0/(2.0*T), n/2)
signal_f = scipy.fftpack.fft(signal)
y_f = 2.0/n * np.abs(signal_f[:n/2])
fft_peak_freq = 1.0*np.argmax(y_f)/T/n
unfiltered.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, fft_peak_freq, '', -1, timestamps, signal, f, y_f)) # -1...confidence not implemented
if(self.verbose):
logger.info('FFT peak frequency: %s'% fft_peak_freq)
# Bin frequency into the ones to detect
freq = [x for x in frequencies_to_detect if abs(x-fft_peak_freq)<f_tolerance]
if(freq):
result.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, freq[0], '', -1, [], [], [], [])) # -1...confidence not implemented
if(self.verbose):
logger.info('LED confirmed, frequency: %s'% freq)
else:
logger.info('Could not associate frequency, discarding')
# Plot all signals and FFTs
if(self.ploteverything):
fig, ax1 = plt.subplots()
ax1.plot(timestamps, signal)
fig, ax2 = plt.subplots()
ax2.plot(f,y_f)
plt.show()
plt.imshow(rgb0)
ax = plt.gca()
font = {'family': 'serif',
'color': 'red',
'weight': 'bold',
'size': 16,
}
# Plot all results
if(self.plotfinal):
for r in result.detections:
pos_n = r.pixels_normalized
pos = Vector2D(1.0*pos_n.x*W, 1.0*pos_n.y*H)
ax.add_patch(Rectangle((pos.x-0.5*cell_width, pos.y-0.5*cell_height), cell_width, cell_height, edgecolor="red", linewidth=3, facecolor="none"))
plt.text(pos.x-0.5*cell_width, pos.y-cell_height, str(r.frequency), fontdict=font)
plt.show()
if(self.publisher is not None):
self.debug_msg.led_all_unfiltered = unfiltered
self.debug_msg.state = 0
self.republish()
return result
def detect_led(self,
images,
mask,
frequencies_to_detect,
min_distance_between_LEDs_pixels):
assert len(images.shape) == 1
n = images.shape[0]
if n == 0:
raise ValueError('No images provided')
timestamps = images['timestamp']
rgb = images['rgb']
rgb0 = rgb[0]
if not mask.shape == rgb0.shape:
raise ValueError('Invalid mask')
if not isinstance(frequencies_to_detect, list):
raise ValueError(frequencies_to_detect)
if not min_distance_between_LEDs_pixels > 0:
raise ValueError(min_distance_between_LEDs_pixels)
channel = images['rgb'][:,:,:,0] # just using first channel
cell_width = 15
cell_height = 15
(cell_vals, crop_offset) = self.downsample(channel, cell_width, cell_height)
candidates_mask = self.get_candidate_cells(cell_vals, 100)
candidate_cells = [(i,j) for (i,j) in np.ndindex(candidates_mask.shape) if candidates_mask[i,j]]
# Create result object
result = LEDDetectionArray()
# Detect frequencies and discard non-periodic signals
# ts_tolerance = 0.2 # unnecessary
f_tolerance = 0.25
min_num_periods = 3
for (i,j) in candidate_cells:
signal = cell_vals[:,i,j]
signal = signal-np.mean(signal)
zero_crossings = np.where(np.diff(np.sign(signal)))[0]
zero_crossings_t = timestamps[zero_crossings]
led_img_coords = Vector2D((0.5+j)*cell_width+crop_offset[1], (0.5+i)*cell_height+crop_offset[0])
diffs = [b-a for a, b in zip(zero_crossings_t, zero_crossings_t[1:])]
if(self.verbose):
logger.info('Coords: %s, %s'% (led_img_coords.x,led_img_coords.y))
logger.info('Zero crossings: %s'%zero_crossings_t)
logger.info('Diffs: %s'%diffs)
logger.info('Zero-crossing measured freq %s'% (0.5/np.mean(diffs)))
if(len(zero_crossings)<min_num_periods):
if(self.verbose):
logger.info('Not an LED, discarded\n')
continue
# Frequency estimation based on zero crossings - quite bad
#for f in frequencies_to_detect:
# if(all(d-ts_tolerance <= 0.5/f <= d+ts_tolerance for d in diffs)):
# if(self.verbose):
# logger.info('Confirmed LED with frequency %s\n'%f)
# recover coordinates of centroid
# result.detections.append(LEDDetection(timestamps[0], timestamps[-1],
# led_img_coords, f, '', -1)) # -1...confidence not implemented
# break
# Frequency estimation based on FFT
T = 1.0/30 # TODO expecting 30 fps, but RESAMPLE to be sure
f = np.linspace(0.0, 1.0/(2.0*T), n/2)
signal_f = scipy.fftpack.fft(signal)
y_f = 2.0/n * np.abs(signal_f[:n/2])
fft_peak_freq = 1.0*np.argmax(y_f)/T/n
if(self.verbose):
logger.info('FFT peak frequency: %s'% fft_peak_freq)
# Bin frequency into the ones to detect
freq = [x for x in frequencies_to_detect if abs(x-fft_peak_freq)<f_tolerance]
if(freq):
result.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
rospy.Time.from_sec(timestamps[-1]), led_img_coords, freq[0], '', -1)) # -1...confidence not implemented
if(self.verbose):
logger.info('LED confirmed, frequency: %s'% freq)
else:
logger.info('Could not associate frequency, discarding')
print(signal.shape)
print(timestamps[:15].shape)
# Plot all signals and FFTs
if(self.ploteverything):
fig, ax1 = plt.subplots()
ax1.plot(timestamps[:15], signal)
fig, ax2 = plt.subplots()
ax2.plot(f[:15],y_f)
plt.show()
plt.imshow(rgb0)
ax = plt.gca()
font = {'family': 'serif',
'color': 'red',
'weight': 'bold',
'size': 16,
}
# Plot all results
if(self.plotfinal):
for r in result.detections:
pos = r.pixels_normalized
ax.add_patch(Rectangle((pos.x-0.5*cell_width, pos.y-0.5*cell_height), cell_width, cell_height, edgecolor="red", linewidth=3, facecolor="none"))
plt.text(pos.x-0.5*cell_width, pos.y-cell_height, str(r.frequency), fontdict=font)
plt.show()
return result
def detect_led(self, images, mask, frequencies_to_detect,
min_distance_between_LEDs_pixels):
assert len(images.shape) == 1
n = images.shape[0]
if n == 0:
raise ValueError('No images provided')
timestamps = images['timestamp']
rgb = images['rgb']
rgb0 = rgb[0]
if not mask.shape == rgb0.shape:
raise ValueError('Invalid mask')
if not isinstance(frequencies_to_detect, list):
raise ValueError(frequencies_to_detect)
if not min_distance_between_LEDs_pixels > 0:
raise ValueError(min_distance_between_LEDs_pixels)
# tuneable parameters
partitions_x = 20
partitions_y = 20
intensity_variance_threshold = 500 #TODO: this is arbitrary
# should be 480x640?
W = rgb0.shape[0]
H = rgb0.shape[1]
partition_width = W / partitions_x
partition_height = H / partitions_y
# create result object
result = LEDDetectionArray()
partition_intensity_data = []
with open('raw_data_argo1.json', 'w') as f:
numpy.savez(f,
images=images,
mask=mask,
frequencies_to_detect=frequencies_to_detect,
min_distance_between_LEDs_pixels=
min_distance_between_LEDs_pixels)
return 0
# jump by partition
for x in range(0, W, partition_width):
for y in range(0, H, partition_height):
logger.info('Looking at partition %s %s' % (x, y))
# look at same partition across multiple images
average_intensities = []
for image in images['rgb']:
intensities = []
# loop through pixels in each partition
for x_coord in range(x, x + partition_width):
for y_coord in range(y, y + partition_height):
if x_coord > W or y_coord > H:
continue
pixel = image[x_coord][y_coord]
# a simple approach from http://stackoverflow.com/questions/596216/formula-to-determine-brightness-of-rgb-color
# could also use matplotlib's rgb_to_hsv?
# TODO: check if pixel order is RGB or BGR (assuming RGB)
#intensity = 0.299*pixel[0] + 0.587*pixel[1] + 0.114*pixel[2]
#intensity = pixel[0] # red channel
#intensity = pixel[1] # green channel
intensity = pixel[2] # blue channel
intensities.append(intensity)
average_intensities.append(numpy.amax(intensities))
partition_intensity_data.append(average_intensities)
variance = numpy.var(average_intensities)
if variance > intensity_variance_threshold:
#threshold into "on" and "off" states
pass
def main():
script_name = os.path.basename(sys.argv[0])
args = sys.argv[1:]
if len(args) != 2:
msg = """
Usage:
rosrun led_detection <script> <tests> <algorithms>
where:
<tests> = comma separated list of algorithms. May use "*".
<algorithms> = comma separated list of algorithms. May use "*".
For example, this runs all tests on all algorithms:
rosrun led_detection <script> '*' '*'
The default algorithm is called "baseline", and the tests are invoked using:
rosrun led_detection <script> '*' 'baseline'
"""
msg = msg.replace('<script>', script_name)
logger.error(msg)
sys.exit(2)
which_tests0 = sys.argv[1]
which_estimators0 = sys.argv[2]
root = os.environ['DUCKIETOWN_ROOT']
dirname = 'catkin_ws/src/f23-LED/led_detection/scripts/'
filename = 'all_tests.yaml'
filename = os.path.join(root, dirname, filename)
alltests = load_tests(filename)
estimators = { 'baseline' :
LEDDetector(ploteverything=False, verbose=True, plotfinal=False),
'LEDDetector_plots' : LEDDetector(True, True, True)}
#,'LEDDetector_forloops' : LEDDetector_forloops(True, True, True)}
which_tests = expand_string(which_tests0, list(alltests))
which_estimators = expand_string(which_estimators0, list(estimators))
logger.info(' tests: %r |-> %s' % (which_tests0, which_tests))
logger.info('estimators: %r |-> %s' % (which_estimators0, which_estimators))
# which tests to execute
test_results = {}
for id_test in which_tests:
for id_estimator in which_estimators:
result = run_test(id_test, alltests[id_test], id_estimator, estimators[id_estimator])
test_results[(id_test, id_estimator)] = result
nfailed = list(test_results.values()).count(False)
if not nfailed:
logger.info('All tests passed')
else:
which = [k for k, v in test_results.items() if not v]
logger.error('These tests failed: %s ' % which)
sys.exit(3)
def detect_led(self,
images,
frequencies_to_detect,
cell_size,
crop_rect_norm=[0,0,1.0,1.0]):
assert len(images.shape) == 1
n = images.shape[0]
if n == 0:
raise ValueError('No images provided')
timestamps = images['timestamp']
rgb = images['rgb']
H, W, _ = rgb[0].shape
if not isinstance(frequencies_to_detect, list):
raise ValueError(frequencies_to_detect)
if(self.publisher is not None):
self.debug_msg.cell_size = cell_size
self.debug_msg.crop_rect_norm = crop_rect_norm
self.republish()
# Crop + Greyscale
tlx = floor(1.0*W*crop_rect_norm[0])
tly = floor(1.0*H*crop_rect_norm[1])
brx = ceil(1.0*W*crop_rect_norm[2])
bry = ceil(1.0*H*crop_rect_norm[3])
croppedshape = [images['rgb'].shape[0], bry-tly, brx-tlx]
channel = np.zeros(croppedshape)
for i in range(n):
channel[i,:,:] = cv2.cvtColor(images['rgb'][i,tly:bry,tlx:brx,:], cv2.COLOR_BGR2GRAY)
print('expected shape {0}'.format(croppedshape))
print('channel.shape {0}'.format(channel.shape))
cell_width = cell_size[0]
cell_height = cell_size[1]
var_threshold = 100
(cell_vals, crop_offset) = self.downsample(channel, cell_width, cell_height)
candidates_mask = self.get_candidate_cells(cell_vals, var_threshold)
candidate_cells = [(i,j) for (i,j) in np.ndindex(candidates_mask.shape) if candidates_mask[i,j]]
if(self.publisher is not None):
for idx in candidate_cells:
self.debug_msg.candidates.append(Vector2D(idx[0]+crop_offset[1]+tly, idx[1]+crop_offset[0]+tlx))
# Create result object
result = LEDDetectionArray()
unfiltered = LEDDetectionArray()
# Detect frequencies and discard non-periodic signals
f_tolerance = 0.3
for (i,j) in candidate_cells:
signal = cell_vals[:,i,j]
signal = signal-np.mean(signal)
led_img_coords = Vector2D((0.5+j)*cell_width+crop_offset[1]+tlx, (0.5+i)*cell_height+crop_offset[0]+tly)
led_img_coords_norm = Vector2D(1.0*led_img_coords.x/W, 1.0*led_img_coords.y/H)
if(self.verbose):
logger.info('Coords: %s, %s'% (led_img_coords.x,led_img_coords.y))
# Frequency estimation based on FFT
T = 1.0/30 # TODO expecting 30 fps, but RESAMPLE to be sure
f = np.linspace(0.0, 1.0/(2.0*T), n/2)
signal_f = scipy.fftpack.fft(signal)
y_f = 2.0/n * np.abs(signal_f[:n/2])
fft_peak_freq = 1.0*np.argmax(y_f)/T/n
unfiltered.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, fft_peak_freq, '', -1, timestamps, signal, f, y_f)) # -1...confidence not implemented
if(self.verbose):
logger.info('FFT peak frequency: %s'% fft_peak_freq)
# Bin frequency into the ones to detect
freq = [x for x in frequencies_to_detect if abs(x-fft_peak_freq)<f_tolerance]
if(freq):
result.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, freq[0], '', -1, [], [], [], [])) # -1...confidence not implemented
if(self.verbose):
logger.info('LED confirmed, frequency: %s'% freq)
else:
logger.info('Could not associate frequency, discarding')
# Plot all signals and FFTs
if(self.ploteverything):
fig, ax1 = plt.subplots()
ax1.plot(timestamps, signal)
fig, ax2 = plt.subplots()
ax2.plot(f,y_f)
plt.show()
if(self.ploteverything):
plt.imshow(rgb[0])
ax = plt.gca()
font = {'family': 'serif',
'color': 'red',
'weight': 'bold',
'size': 16,
}
# Plot all results
if(self.plotfinal):
for r in result.detections:
pos_n = r.pixels_normalized
pos = Vector2D(1.0*pos_n.x*W, 1.0*pos_n.y*H)
ax.add_patch(Rectangle((pos.x-0.5*cell_width, pos.y-0.5*cell_height), cell_width, cell_height, edgecolor="red", linewidth=3, facecolor="none"))
plt.text(pos.x-0.5*cell_width, pos.y-cell_height, str(r.frequency), fontdict=font)
plt.show()
if(self.publisher is not None):
self.debug_msg.led_all_unfiltered = unfiltered
self.debug_msg.state = 0
self.republish()
return result
def detect_led(self,
images,
mask,
frequencies_to_detect,
min_distance_between_LEDs_pixels):
assert len(images.shape) == 1
n = images.shape[0]
if n == 0:
raise ValueError('No images provided')
timestamps = images['timestamp']
rgb = images['rgb']
rgb0 = rgb[0]
if not mask.shape == rgb0.shape:
raise ValueError('Invalid mask')
if not isinstance(frequencies_to_detect, list):
raise ValueError(frequencies_to_detect)
if not min_distance_between_LEDs_pixels > 0:
raise ValueError(min_distance_between_LEDs_pixels)
# tuneable parameters
partitions_x = 20
partitions_y = 20
intensity_variance_threshold = 500 #TODO: this is arbitrary
# should be 480x640?
W = rgb0.shape[0]
H = rgb0.shape[1]
partition_width = W / partitions_x
partition_height = H / partitions_y
# create result object
result = LEDDetectionArray()
partition_intensity_data = []
with open('raw_data_argo1.json','w') as f:
numpy.savez(f, images=images, mask=mask,
frequencies_to_detect=frequencies_to_detect, min_distance_between_LEDs_pixels=min_distance_between_LEDs_pixels)
return 0
# jump by partition
for x in range(0, W, partition_width):
for y in range(0, H, partition_height):
logger.info('Looking at partition %s %s' % (x, y))
# look at same partition across multiple images
average_intensities = []
for image in images['rgb']:
intensities = []
# loop through pixels in each partition
for x_coord in range(x, x+partition_width):
for y_coord in range(y,y+partition_height):
if x_coord > W or y_coord > H:
continue
pixel = image[x_coord][y_coord]
# a simple approach from http://stackoverflow.com/questions/596216/formula-to-determine-brightness-of-rgb-color
# could also use matplotlib's rgb_to_hsv?
# TODO: check if pixel order is RGB or BGR (assuming RGB)
#intensity = 0.299*pixel[0] + 0.587*pixel[1] + 0.114*pixel[2]
#intensity = pixel[0] # red channel
#intensity = pixel[1] # green channel
intensity = pixel[2] # blue channel
intensities.append(intensity)
average_intensities.append(numpy.amax(intensities))
partition_intensity_data.append(average_intensities)
variance = numpy.var(average_intensities)
if variance > intensity_variance_threshold:
#threshold into "on" and "off" states
pass