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()