def __init__(self, configuration):
param_names = [
"localization_template",
"delta_segment",
"variables",
"F",
"optimize",
"bounds_theta_deg",
"precision",
]
dtu.Configurable.__init__(self, param_names, configuration)
self.grid_helper = GridHelper(dict(self.variables), precision=self.precision)
self.initialize_belief()
self.last_segments_used = None
self.delta_segment = float(self.delta_segment)
def __init__(self, configuration):
param_names = [
'localization_template',
'delta_segment',
'variables',
'F',
'optimize',
'bounds_theta_deg',
'precision',
]
dtu.Configurable.__init__(self, param_names, configuration)
self.grid_helper = GridHelper(OrderedDict(self.variables),
precision=self.precision)
self.initialize_belief()
self.last_segments_used = None
self.delta_segment = float(self.delta_segment)
class LaneFilterMoreGeneric(dtu.Configurable, LaneFilterInterface):
""" """
localization_template: str
_localization_template: LocalizationTemplate
variables: dict
precision: float
belief: np.ndarray
optimize: bool
bounds_theta_deg: List[float]
F: object
rep_map: "PNRep"
def __init__(self, configuration):
param_names = [
"localization_template",
"delta_segment",
"variables",
"F",
"optimize",
"bounds_theta_deg",
"precision",
]
dtu.Configurable.__init__(self, param_names, configuration)
self.grid_helper = GridHelper(dict(self.variables), precision=self.precision)
self.initialize_belief()
self.last_segments_used = None
self.delta_segment = float(self.delta_segment)
def initialize_belief(self):
self.belief = self.grid_helper.create_new()
n = self.belief.shape[0] * self.belief.shape[1]
self.belief.fill(1.0 / n)
assert_almost_equal(self.belief.flatten().sum(), 1.0)
def initialize(self):
easy_algo_db = get_easy_algo_db()
self._localization_template = easy_algo_db.create_instance(
FAMILY_LOC_TEMPLATES, self.localization_template
)
self.initialize_belief()
sm = self._localization_template.get_map()
self.rep_map = get_compat_representation_map(sm, self.delta_segment)
def predict(self, dt, v, w):
pass
def get_status(self):
# TODO
return LaneFilterInterface.GOOD
def getStatus(self):
return self.get_status()
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 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 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 get_estimate(self):
# TODO: make F parameter
return self.grid_helper.get_max_weighted(self.belief, F=1)
@dtu.deprecated("use get_estimate")
def getEstimate(self):
""" Returns a list with two elements: (d, phi) """
res = self.get_estimate()
return [res["d"], res["phi"]]
def getMax(self):
return self.belief.max()
def get_entropy(self):
s = entropy(self.belief.flatten())
return s
def generate_votes(self, segment, delta):
"""
yields xytheta, weight
"""
p1 = np.array([segment.points[0].x, segment.points[0].y, segment.points[0].z])
p2 = np.array([segment.points[1].x, segment.points[1].y, segment.points[1].z])
p_hat = 0.5 * p1 + 0.5 * p2
d = np.linalg.norm(p1 - p2)
weight = d
n_hat = get_normal_outward_for_segment(p2, p1)
# SegmentsMap
sm = self._localization_template.get_map()
num = 0
for map_segment in sm.segments:
if map_segment.color == segment.color:
for p, n in iterate_segment_sections(sm, map_segment, delta):
xy, theta = get_estimate(p, n, p_hat, n_hat)
pose = SE2_from_translation_angle(xy, theta)
yield pose, weight
num += 1
if num == 0:
msg = f"No segment found for {segment.color}"
dtu.logger.debug(msg)
def get_plot_phi_d(self, ground_truth=None,
bgcolor: dtu.RGBColor8 = dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW):
facecolor = dtu.matplotlib_01_from_rgb(bgcolor)
figure_args = dict(facecolor=facecolor)
a = dtu.CreateImageFromPylab(dpi=120, figure_args=figure_args)
gh = self.grid_helper
with a as pylab:
grid_helper_plot_field(gh, self.belief, pylab)
grid_helper_annotate_axes(gh, pylab)
estimate = self.get_estimate()
if ground_truth is not None:
ground_truth_location = self._localization_template.coords_from_pose(ground_truth)
grid_helper_mark_point(gh, pylab, ground_truth_location, color="green", markersize=4)
grid_helper_mark_point(gh, pylab, estimate, color="magenta", markersize=10)
s = ""
s += f"status = {self.get_status()}"
for name, spec in zip(gh._names, gh._specs):
convert = lambda x: "%.2f %s" % (
convert_unit(x, spec.units, spec.units_display),
spec.units_display,
)
s += "\n"
s += f"\nest {name} = {convert(estimate[name])}"
if ground_truth is not None:
s += f"\ntrue {name} = {convert(ground_truth_location[name])}"
err = np.abs(ground_truth_location[name] - estimate[name])
s += f"\nabs err = {convert(err)}"
cell = spec.resolution
percent = 100.0 / cell * err
s += f"\nrel err = {percent:.1f} % of cell"
s += "\n true = green dot"
s += "\n"
s += f"\nentropy = {self.get_entropy():.4f}"
s += f"\nmax = {self.belief.max():.4f}"
s += f"\nmin = {self.belief.min():.4f}"
pylab.annotate(s, xy=(0.7, 0.45), xycoords="figure fraction")
grid_helper_set_axes(gh, pylab)
return a.get_bgr()