def detectLines(self, color):
with dtu.timeit_clock('_getMask'):
mask = self._getMask(color)
# Detect yellow points using ST keypoints.
# if color == 'yellow':
# # Default keypoints
# lines = []
# normals = []
# centers = []
# with dtu.timeit_clock('_getKeypoints'):
# kps = self._getKeypoints(mask, color)
# # If kps available, package up lines
# if len(kps) > 0:
# # KP info is stored in [0:1], add +2 for visualization
# lines = np.hstack((kps, kps / 2)).astype('int32')
# # Fake the normals and centres, we don't use these
# normals = np.zeros((len(lines), 2))
# centers = np.zeros((len(lines), 2))
# Detect white points using prob. Hough lines
# Also for red points, for now
if color == 'white' or color == 'red' or color == 'yellow':
with dtu.timeit_clock('_getLines'):
lines, normals, centers = self._getLines(mask)
return Detections(lines=lines, normals=normals, area=mask, centers=centers)
def setImage(self, bgr):
with dtu.timeit_clock('np.copy'):
self.bgr = np.copy(bgr)
with dtu.timeit_clock('cvtColor COLOR_BGR2HSV'):
self.hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
with dtu.timeit_clock('_findEdge'):
self.edges = self._findEdge(self.bgr)
def detectLines(self, color):
with dtu.timeit_clock('_colorFilter'):
bw, edge_color = self._colorFilter(color)
with dtu.timeit_clock('_HoughLine'):
lines = self._HoughLine(edge_color)
with dtu.timeit_clock('_findNormal'):
centers, normals = self._findNormal(bw, lines)
return Detections(lines=lines, normals=normals, area=bw, centers=centers)
def setImage(self, bgr):
# Smooth image, convert to hsv, and convert to grayscale.
with dtu.timeit_clock('np.copy'):
self.bgr = np.copy(bgr)
with dtu.timeit_clock('bilateral smoothing'):
self.smoothed = self._smooth(bgr)
with dtu.timeit_clock('cvtColor COLOR_BGR2HSV'):
self.hsv = cv2.cvtColor(self.smoothed, cv2.COLOR_BGR2HSV)
with dtu.timeit_clock('_greyscale'):
self.gray = cv2.cvtColor(self.smoothed, cv2.COLOR_BGR2GRAY)
def phase(self, phase_name):
with dtu.timeit_clock(phase_name):
if not phase_name in self.phase_names:
self.phase_names.append(phase_name)
if self.last_msg_being_processed is None:
msg = 'Did not call decided_to_process() before?'
raise ValueError(msg)
# t1 = rospy.get_time() # @UndefinedVariable
t1 = time.time()
c1 = time.clock()
try:
yield
finally:
c2 = time.clock()
# t2 = rospy.get_time() # @UndefinedVariable
t2 = time.time()
delta_clock = c2 - c1
delta_wall = t2 - t1
latency_from_acquisition = t2 - self.last_msg_being_processed
self.stats[(phase_name, 'clock')].sample(delta_clock)
self.stats[(phase_name, 'wall')].sample(delta_wall)
self.stats[(phase_name,
'latency')].sample(latency_from_acquisition)
def update(self, segment_list):
""" Returns the likelihood """
self.last_segments_used = segment_list.segments
with dtu.timeit_clock('generating likelihood'):
if not self.optimize:
measurement_likelihood = self.generate_measurement_likelihood(
segment_list.segments)
else:
measurement_likelihood = self.generate_measurement_likelihood_faster(
segment_list.segments)
with dtu.timeit_clock('multiply belief'):
if measurement_likelihood is not None:
self.belief = np.multiply(self.belief, measurement_likelihood)
if np.sum(self.belief) == 0:
self.belief = measurement_likelihood
alpha = 1.0 / np.sum(self.belief)
self.belief = self.belief * alpha
return measurement_likelihood
#!/usr/bin/env python
import duckietown_utils as dtu
import sys
dtu.DuckietownConstants.show_timeit_benchmarks = True
with dtu.timeit_clock('empty 1'):
with dtu.timeit_clock('empty 2'):
with dtu.timeit_clock('empty 3'):
with dtu.timeit_clock('empty 4'):
with dtu.timeit_clock('empty 5'):
with dtu.timeit_clock('empty 6'):
with dtu.timeit_clock('empty 7'):
with dtu.timeit_clock('empty 8'):
with dtu.timeit_clock('empty 9'):
pass
def generate_measurement_likelihood_faster(self, segments):
with dtu.timeit_clock("get_compat_representation_obs (%d segments)" %
len(segments)):
rep_obs = get_compat_representation_obs(segments)
rep_map = self.rep_map
with dtu.timeit_clock("generate_votes_faster (map: %d, obs: %d)" %
(len(rep_map.weight), len(rep_obs.weight))):
votes = generate_votes_faster(rep_map, rep_obs)
if self.bounds_theta_deg is not None:
weight = votes.weight.copy()
theta_min = np.deg2rad(self.bounds_theta_deg[0])
theta_max = np.deg2rad(self.bounds_theta_deg[1])
num_outside = 0
for i in range(weight.shape[0]):
theta = votes.theta[i]
if not (theta_min <= theta <= theta_max):
weight[i] = 0
num_outside += 1
# print('Removed %d of %d because outside box' % (num_outside, len(weight)))
votes = remove_zero_weight(votes._replace(weight=weight))
with dtu.timeit_clock("compute pos iterative (%d)" %
len(votes.weight)):
locations = self._localization_template.coords_from_position_orientation(
votes.p, votes.theta)
num = len(locations)
est = np.zeros((2, num))
for i in range(2):
v = list(self.variables)[i]
est[i, :] = locations[v]
F = self.F
compare = False
with dtu.timeit_clock("add voting faster (%d)" % len(votes.weight)):
measurement_likelihood = self.grid_helper.create_new('float32')
measurement_likelihood.fill(0)
if compare:
counts1 = np.zeros(measurement_likelihood.shape, dtype='int')
else:
counts1 = None
added = self.grid_helper.add_vote_faster(measurement_likelihood,
est,
votes.weight,
F=F,
counts=counts1)
# print('Counts (new):\n' + array_as_string(counts1, lambda x: ' %3d' % x))
if compare:
with dtu.timeit_clock("add voting (traditional)"):
measurement_likelihood_classic = self.grid_helper.create_new(
'float32')
measurement_likelihood_classic.fill(0)
hit = miss = 0
counts2 = np.zeros(measurement_likelihood.shape, dtype='int')
for i in range(len(locations)):
loc = locations[i]
weight = votes.weight[i]
value = dict((k, loc[k]) for k in self.variables)
added = self.grid_helper.add_vote(
measurement_likelihood_classic,
value,
weight,
F=F,
counts=counts2)
hit += added > 0
miss += added == 0
# dtu.logger.debug('tradoitional hit: %s miss : %s' % (hit, miss))
# print('Counts (old):\n' + array_as_string(counts2, lambda x: ' %3d' % x))
#
diff = measurement_likelihood - measurement_likelihood_classic
deviation = np.max(np.abs(diff))
if deviation > 1e-6:
s = array_as_string_sign(diff)
print('max deviation: %s' % deviation)
print s
return measurement_likelihood
def generate_measurement_likelihood(self, segments):
# initialize measurement likelihood to all zeros
measurement_likelihood = self.grid_helper.create_new('float32')
measurement_likelihood.fill(0)
hit = miss = 0
pose_weight = []
num_by_color = defaultdict(lambda: 0)
adjust_by_number = False # add to configuration
with dtu.timeit_clock("pose gen for %s segs" % len(segments)):
for segment in segments:
num_by_color[segment.color] += 1
for segment in segments:
for pose, weight in self.generate_votes(
segment, self.delta_segment):
if self.bounds_theta_deg is not None:
theta_deg = np.rad2deg(
dtu.norm_angle(dtu.geo.angle_from_SE2(pose)))
m, M = self.bounds_theta_deg
if not (m <= theta_deg < M):
continue
if adjust_by_number:
n = num_by_color[segment.color]
weight_adjusted = weight * 1.0 / n
else:
weight_adjusted = weight
pose_weight.append((pose, weight_adjusted))
values = []
with dtu.timeit_clock("generating coords for %s votes" %
len(pose_weight)):
for pose, weight in pose_weight:
est = self._localization_template.coords_from_pose(pose)
value = dict((k, float(est[k])) for k in self.variables)
values.append((value, weight))
with dtu.timeit_clock("add voting for %s votes" % len(pose_weight)):
for value, weight in values:
# if value['dstop'] > 0.3:
# print('value:%s' % value)
# else:
# continue
added = self.grid_helper.add_vote(measurement_likelihood,
value,
weight,
F=self.F)
hit += added > 0
miss += added == 0
dtu.logger.debug('hit: %s miss : %s' % (hit, miss))
if np.linalg.norm(measurement_likelihood) == 0:
return None
return measurement_likelihood
def add_vote_faster(self, target, values, weights, F=1, counts=None):
with dtu.timeit_clock("adding additional votes (orig: %s)" %
values.shape[1]):
_factor, values_ref, values, weights, group = self.multiply(
values, weights, F)
# cells_in_group = np.zeros(len(group))
# for i in range(len(group)):
# cells_in_group[group[i]] += 1
# print('cells_in_group: %s' % cells_in_group)
# diff = values - values_ref
# for a in [0,1]:
# print('diff %s min %s max %s res %s' % (a, np.min(diff[a,:]), np.max(diff[a,:]),
# self._specs[a].resolution))
with dtu.timeit_clock("computing coordinates (nvalid = %s)" %
values.shape[1]):
nvalid = values.shape[1]
coords = np.zeros((2, nvalid), dtype='int32')
K = [self.K0, self.K1]
for a in range(2):
spec = self._specs[a]
inv_resolution = 1.0 / spec.resolution
x = (values[a, :] - spec.min) * inv_resolution
# print('values[%d] = %s' % (a, values[a, :]))
# print('x = %s' % x)
coords[a, :] = np.floor(x)
# print('coords[%d] = %s' % (a, coords[a, :]))
xr = (values_ref[a, :] - spec.min) * inv_resolution
dc = (xr - (coords[a, :] + 0.5))
# print('dc[%d] = %s' % (a, dc))
dr = dc * spec.resolution
k = 1
k = K[a](dr)
weights *= k
if nvalid == 0:
return 0
with dtu.timeit_clock("normalizing weights"):
ngroups = np.max(group) + 1
weight_group = np.zeros(ngroups)
if False:
for i in range(len(weights)):
weight_group[group[i]] += weights[i]
else:
np.add.at(weight_group, group, weights)
assert len(weights) == nvalid
weights_normalized = weights / weight_group[group]
with dtu.timeit_clock("selecting valid (using %d)" % values.shape[1]):
AND = np.logical_and
inside0 = AND(self._specs[0].min <= values[0, :],
values[0, :] <= self._specs[0].max)
inside1 = AND(self._specs[1].min <= values[1, :],
values[1, :] <= self._specs[1].max)
inside = AND(inside0, inside1)
# hits = np.sum(inside)
# misses = len(inside) - hits
# print('num hit: %s (eq %d) misses %s (eq %d) ' % (hits, hits/_factor, misses, misses/_factor))
# only consider inside
values = values[:, inside]
values_ref = values_ref[:, inside]
weights = weights[inside]
weights_normalized = weights_normalized[inside]
group = group[inside]
coords = coords[:, inside]
with dtu.timeit_clock("using numpy.at"):
np.add.at(target, tuple(coords), weights_normalized)
if counts is not None:
with dtu.timeit_clock("update counts"):
for i in range(nvalid):
counts[coords[0, i], coords[1, i]] += 1
return nvalid
#!/usr/bin/env python
import duckietown_utils as dtu
import sys
print('starting')
fn = sys.argv[1]
data = open(fn).read()
dtu.DuckietownConstants.show_timeit_benchmarks = True
for f in [
dtu.rgb_from_jpg_by_PIL, dtu.rgb_from_jpg_by_JPEG_library,
dtu.bgr_from_jpg
]:
for i in range(3):
res = f(data)
with dtu.timeit_clock(f.__name__):
for i in range(10):
res = f(data)
print res.shape
