def update(self, segment_list: SegmentList):
""" 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)
check_no_nans(measurement_likelihood)
with dtu.timeit_clock("multiply belief"):
if measurement_likelihood is not None:
s_m = np.sum(measurement_likelihood)
if s_m == 0:
logger.warning("flat likelihood - not updating")
else:
self.belief = np.multiply(self.belief, measurement_likelihood)
s = np.sum(self.belief)
if s == 0:
logger.warning("flat belief, just use likelihood")
self.belief = measurement_likelihood
alpha = 1.0 / s
self.belief = self.belief * alpha
return measurement_likelihood
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 generate_measurement_likelihood(self, segments: List[Segment]):
# 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(f"pose gen for {len(segments)} segs"):
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(f"generating coords for {len(pose_weight)} votes"):
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(f"add voting for {len(pose_weight)} votes"):
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(f"hit: {hit} miss : {miss}")
if np.linalg.norm(measurement_likelihood) == 0:
return None
return measurement_likelihood
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 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()
t1 = time.time()
c1 = time.process_time()
try:
yield
finally:
c2 = time.process_time()
# t2 = rospy.get_time()
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 generate_measurement_likelihood_faster(self, segments: List[Segment]):
with dtu.timeit_clock(f"get_compat_representation_obs ({len(segments)} segments)"):
rep_obs = get_compat_representation_obs(segments)
rep_map = self.rep_map
with dtu.timeit_clock(
f"generate_votes_faster (map: {len(rep_map.weight)}, obs: {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(f"compute pos iterative ({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(f"add voting faster ({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(f"max deviation: {deviation}")
print(s)
return measurement_likelihood
def add_vote_faster(self, target, values, weights, F=1, counts=None):
with dtu.timeit_clock(
f"adding additional votes (orig: {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(
f"computing coordinates (nvalid = {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(f"selecting valid (using {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
