def name_from_spec(x: Union[str, dict]) -> str: dtu.check_isinstance(x, (str, dict)) if isinstance(x, str): return x elif isinstance(x, dict): name, _ = _parse_inline_spec(x) return name
def create_instance(self, family_name: str, instance_name_or_spec: Union[dict, str]): dtu.check_isinstance(instance_name_or_spec, (dict, str)) family = self.get_family(family_name) if not family.valid: msg = f"Cannot instantiate {instance_name_or_spec!r} because its family {family_name!r} is " \ f"invalid." msg += "\n\n" + dtu.indent(family.error_if_invalid, " > ") raise dtu.DTConfigException(msg) if isinstance(instance_name_or_spec, str): instance_name = instance_name_or_spec dtu.check_is_in("instance", instance_name, family.instances) instance = family.instances[instance_name] if not instance.valid: msg = f"Cannot instantiate {instance_name!r} because it is invalid:\n" \ f"{dtu.indent(instance.error_if_invalid, '> ')}" raise dtu.DTConfigException(msg) res = dtu.instantiate(instance.constructor, instance.parameters) elif isinstance(instance_name_or_spec, dict): _name, spec = _parse_inline_spec(instance_name_or_spec) res = dtu.instantiate(spec.constructor, spec.parameters) else: assert False interface = dtu.import_name(family.interface) if not isinstance(res, interface): msg = f"I expected that {instance_name_or_spec!r} would be a {interface.__name__} but it is a " \ f"{type(res).__name__}." raise dtu.DTConfigException(msg) return res
def read_data_for_signals(bag_in, prefix_in, signals): topic2messages = defaultdict(lambda: dict(timestamp=[], data=[])) topics = [] for signal_spec in signals: dtu.check_isinstance(signal_spec, PlotSignalSpec) topic_fqn = prefix_in + signal_spec.topic topics.append(topic_fqn) for _j, mp in enumerate(bag_in.read_messages_plus(topics=topics)): data = interpret_ros(mp.msg) topic2messages[mp.topic]["data"].append(data) topic2messages[mp.topic]["timestamp"].append( mp.time_from_physical_log_start) for v in list(topic2messages.values()): v["data"] = np.array(v["data"]) v["timestamp"] = np.array(v["timestamp"]) for signal_spec in signals: topic_fqn = prefix_in + signal_spec.topic if not topic_fqn in topic2messages: msg = f"Could not found any value for topic {topic_fqn!r}." raise ValueError(msg) return topic2messages
def _parse_inline_spec(x: dict) -> Tuple[str, SpecValues]: dtu.check_isinstance(x, dict) if len(x) != 1: msg = f"Invalid spec: length is {len(x)}" dtu.raise_desc(ValueError, msg, x=x) key = list(x)[0] value = x[key] value = copy.deepcopy(value) if "description" in value: description = value.pop("description") dtu.dt_check_isinstance("description", description, str) else: description = None constructor = value.pop("constructor") dtu.dt_check_isinstance("constructor", constructor, str) parameters = value.pop("parameters") dtu.dt_check_isinstance("parameters", parameters, (dict, type(None))) if parameters is None: parameters = {} res = SpecValues(constructor=constructor, parameters=parameters, description=description) return key, res
def yaml_representation_of_phy_logs(logs: Dict[str, PhysicalLog]) -> str: dtu.check_isinstance(logs, dict) res = {} for k, l in list(logs.items()): res[k] = yaml_from_physical_log(l) s = dtu.yaml_dump_pretty(res) return s
def logs_from_yaml(data: dict) -> Dict[str, PhysicalLog]: dtu.check_isinstance(data, dict) res = {} for k, d in list(data.items()): res[k] = physical_log_from_yaml(d) return res
def __init__(self, current: ResultDBEntry, entries: Sequence[ResultDBEntry]): for e in entries: dtu.check_isinstance(e, ResultDBEntry) self.entries = entries self.current = current dtu.check_isinstance(current, ResultDBEntry)
def __str__(self): n = len(self.entries) s = f"ResultsDB with {n} entries" s += "\n" + dtu.indent(str(self.current), "", " current: ") for i, p in enumerate(self.entries): dtu.check_isinstance(p, ResultDBEntry) s += "\n" + dtu.indent(str(p), "", f" {i + 1:2d} of {n}: ") return s
def __init__(self, variables: Dict[str, Dict], precision="float32"): self.precision = precision dtu.check_isinstance(variables, dict) if len(variables) != 2: msg = f"I can only deal with 2 variables, obtained {variables}" raise ValueError(msg) self._names = list(variables) self._specs = [spec_from_yaml(variables[_]) for _ in self._names] s0 = self._specs[0] s1 = self._specs[1] self._mgrids = list(np.mgrid[s0.min:s0.max:s0.resolution, s1.min:s1.max:s1.resolution]) for a in [0, 1]: new_max = self._specs[ a].min + self._mgrids[a].shape[a] * self._specs[a].resolution self._specs[a] = replace(self._specs[a], max=new_max) H, W = self.shape = self._mgrids[0].shape self._centers = [np.zeros(shape=(H, W)), np.zeros(shape=(H, W))] for i, j in itertools.product(list(range(H)), list(range(W))): self._centers[0][i, j] = s0.min + (i + 0.5) * s0.resolution self._centers[1][i, j] = s1.min + (j + 0.5) * s1.resolution # these are the bounds you would give to pcolor # there is one row and one column more # self.d, self.phi are the lower corners # Each cell captures this area: # (X[i, j], Y[i, j]), # (X[i, j+1], Y[i, j+1]), # (X[i+1, j], Y[i+1, j]), # (X[i+1, j+1], Y[i+1, j+1]) self._mgrids_plus = list( np.mgrid[s0.min:s0.max + s0.resolution:s0.resolution, s1.min:s1.max + s1.resolution:s1.resolution, ]) k0_o_sigma = 1.0 / self._specs[0].resolution def K0(x): d = x * k0_o_sigma d2 = d * d return np.exp(-d2) k1_o_sigma = 1.0 / self._specs[1].resolution def K1(x): d = x * k1_o_sigma d2 = d * d return np.exp(-d2) self.K0 = K0 self.K1 = K1
def get_homography_for_robot(robot_name: str) -> np.ndarray: dtu.check_isinstance(robot_name, str) # find the file fn = get_homography_info_config_file(robot_name) # load the YAML data = dtu.yaml_load_file(fn, plain_yaml=True) # True means "plain" # convert the YAML homography = homography_from_yaml(data) return homography
def validate(self): for k, p in list(self.points.items()): dtu.check_isinstance(p, SegMapPoint) dtu.check_isinstance(k, str) coords = p.coords if not isinstance(coords, np.ndarray): self.points[k] = replace(p, coords=np.array(coords)) for S in self.segments: dtu.check_isinstance(S, SegMapSegment) for p in S.points: if not p in self.points: msg = f"Invalid point {p!r}" raise ValueError(msg) assert len(S.points) == 2 w1 = self.points[S.points[0]].coords w2 = self.points[S.points[1]].coords dist = np.linalg.norm(w1 - w2) if dist == 0: msg = f"This is a degenerate segment (points: {w1} {w2}) " msg += f"names: {S.points}" raise ValueError(msg) for F in self.faces: dtu.check_isinstance(F, SegMapFace) for p in F.points: if not p in self.points: msg = f"Invalid point {p!r}" raise ValueError(msg)
def download_if_necessary(log: PhysicalLog) -> PhysicalLog: """ Downloads the log if necessary. Use like this: log = ... log2 = download_if_necessary(log) """ # dtu.logger.info('Log:\n%s' % log.resources) dtu.check_isinstance(log, PhysicalLog) resource_name = "bag" filename = get_log_if_not_exists(log, resource_name=resource_name) local_uri = _create_file_uri(filename) resource: PhysicalLogResource = log.resources[resource_name] urls: List[str] = resource["urls"] urls.append(local_uri) return log
def query_results_one(self, branch, date, commit): possible = self.query_results(branch, date, commit) from easy_regression.conditions.eval import DataNotFound if len(possible) == 0: msg = "Could not find any match for the query." msg += f"\n branch: {branch}" msg += f"\n date: {date}" msg += f"\n commit: {commit}" raise DataNotFound(msg) if len(possible) > 1: n = len(possible) msg = f"Found {n} matches for this query." msg += f"\n branch: {branch}" msg += f"\n date: {date}" msg += f"\n commit: {commit}" msg += "\nThese are the matches:" for i, p in enumerate(possible): dtu.check_isinstance(p, ResultDBEntry) msg += "\n" + dtu.indent(str(p), f" {i + 1:2d} of {n}: ") raise AmbiguousQuery(msg) return possible[0]
def run_pipeline( image: dtu.NPImageBGR, gpg: GroundProjectionGeometry, rectifier: Rectify, line_detector_name: str, image_prep_name: str, lane_filter_name: str, anti_instagram_name: str, all_details: bool = False, ground_truth=None, actual_map=None, ) -> Tuple[Dict[str, dtu.NPImageBGR], Dict[str, float]]: """ Image: numpy (H,W,3) == BGR Returns a dictionary, res with the following fields: res['input_image'] ground_truth = pose """ logger.debug(f"backend: {matplotlib.get_backend()}") logger.debug(f"fname: {matplotlib.matplotlib_fname()}") quick: bool = False dtu.check_isinstance(image, np.ndarray) res: Dict[str, dtu.NPImageBGR] = {} stats = {} res["Raw input image"] = image algo_db = get_easy_algo_db() line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR, line_detector_name) lane_filter = algo_db.create_instance(FAMILY_LANE_FILTER, lane_filter_name) image_prep = algo_db.create_instance(ImagePrep.FAMILY, image_prep_name) ai = algo_db.create_instance(AntiInstagramInterface.FAMILY, anti_instagram_name) pts = ProcessingTimingStats() pts.reset() pts.received_message() pts.decided_to_process() if all_details: segment_list = image_prep.process(FakeContext(), image, line_detector, transform=None) res["segments_on_image_input"] = vs_fancy_display( image_prep.image_cv, segment_list) res["segments_on_image_resized"] = vs_fancy_display( image_prep.image_resized, segment_list) with pts.phase("calculate AI transform"): ai.calculateTransform(image) with pts.phase("apply AI transform"): transformed = ai.applyTransform(image) if all_details: res["image_input_transformed"] = transformed with pts.phase("edge detection"): # note: do not apply transform twice! segment_list2 = image_prep.process(pts, image, line_detector, transform=ai.applyTransform) if all_details: res["resized and corrected"] = image_prep.image_corrected logger.debug(f"segment_list2: {len(segment_list2.segments)}") if all_details: res["segments_on_image_input_transformed"] = vs_fancy_display( image_prep.image_cv, segment_list2) if all_details: res["segments_on_image_input_transformed_resized"] = vs_fancy_display( image_prep.image_resized, segment_list2) if all_details: grid = get_grid(image.shape[:2]) res["grid"] = grid res["grid_remapped"] = rectifier.rectify(grid) # res['difference between the two'] = res['image_input']*0.5 + res['image_input_rect']*0.5 with pts.phase("rectify_segments"): segment_list2_rect = rectify_segments(rectifier, gpg, segment_list2) # Project to ground with pts.phase("find_ground_coordinates"): sg = find_ground_coordinates(gpg, segment_list2_rect) lane_filter.initialize() # lane_filter.get_plot_phi_d() if all_details: res["prior"] = lane_filter.get_plot_phi_d() with pts.phase("lane filter update"): logger.info(type(lane_filter).__name__) if type(lane_filter).__name__ == "LaneFilterHistogram": # XXX merging pain _likelihood = lane_filter.update(sg.segments) else: _likelihood = lane_filter.update(sg) if not quick: with pts.phase("lane filter plot"): res["likelihood"] = lane_filter.get_plot_phi_d( ground_truth=ground_truth) easy_algo_db = get_easy_algo_db() if isinstance(lane_filter, LaneFilterMoreGeneric): template_name = lane_filter.localization_template else: template_name = "DT17_template_straight_straight" dtu.logger.debug( f"Using default template {template_name!r} for visualization") localization_template = easy_algo_db.create_instance( FAMILY_LOC_TEMPLATES, template_name) with pts.phase("lane filter get_estimate()"): est = lane_filter.get_estimate() # Coordinates in TILE frame g = localization_template.pose_from_coords(est) tinfo = TransformationsInfo() tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_AXLE, g=g) if all_details: if actual_map is not None: # sm_axle = tinfo.transform_map_to_frame(actual_map, FRAME_AXLE) res["real"] = plot_map_and_segments(actual_map, tinfo, sg.segments, dpi=120, ground_truth=ground_truth) with pts.phase("rectify"): rectified0 = rectifier.rectify(image) rectified = ai.applyTransform(rectified0) if all_details: res["image_input_rect"] = rectified res["segments rectified on image rectified"] = vs_fancy_display( rectified, segment_list2_rect) assumed = localization_template.get_map() if not quick: with pts.phase("plot_map_and_segments"): res["model assumed for localization"] = plot_map_and_segments( assumed, tinfo, sg.segments, dpi=120, ground_truth=ground_truth) assumed_axle = tinfo.transform_map_to_frame(assumed, FRAME_AXLE) with pts.phase("plot_map reprojected"): res["map reprojected on image"] = plot_map( rectified, assumed_axle, gpg, do_ground=False, do_horizon=True, do_faces=False, do_faces_outline=True, do_segments=False, ) with pts.phase("quality computation"): predicted_segment_list_rectified = predict_segments(sm=assumed_axle, gpg=gpg) quality_res, quality_stats = judge_quality( image, segment_list2_rect, predicted_segment_list_rectified) res.update(quality_res) # res['blurred']= cv2.medianBlur(image, 11) stats["estimate"] = est stats.update(quality_stats) dtu.logger.info(pts.get_stats()) return res, stats
def do_plot(bag_in, prefix_in, bag_out, prefix_out, signals, plot_name): topic2messages = defaultdict(lambda: dict(timestamp=[], data=[])) topics = [] for signal_spec in signals: dtu.check_isinstance(signal_spec, PlotSignalSpec) topic_fqn = prefix_in + signal_spec.topic topics.append(topic_fqn) for _j, mp in enumerate(bag_in.read_messages_plus(topics=topics)): data = interpret_ros(mp.msg) topic2messages[mp.topic]["data"].append(data) topic2messages[mp.topic]["timestamp"].append( mp.time_from_physical_log_start) for signal_spec in signals: topic_fqn = prefix_in + signal_spec.topic if not topic_fqn in topic2messages: msg = f"Could not found any value for topic {topic_fqn!r}." raise ValueError(msg) bgcolor = dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW dpi = 100 figure_args = dict(facecolor=dtu.matplotlib_01_from_rgb(bgcolor)) a = dtu.CreateImageFromPylab(dpi=dpi, figure_args=figure_args) use_legend = len(signals) >= 3 # todo: check same units with a as pylab: axes = [] _fig, ax0 = pylab.subplots() ax0.set_xlabel("time (s)") axes.append(ax0) if use_legend: for i in range(len(signals) - 1): axes.append(ax0) else: for i in range(len(signals) - 1): axes.append(ax0.twinx()) for i, signal_spec in enumerate(signals): ax = axes[i] topic_fqn = prefix_in + signal_spec.topic recorded = topic2messages[topic_fqn] data = np.array(recorded["data"]) t = np.array(recorded["timestamp"]) color = signal_spec.color markersize = 5 data_converted = convert_unit(data, signal_spec.units, signal_spec.units_display) ax.plot( t, data_converted, "o", color=color, label=signal_spec.label, markersize=markersize, clip_on=False, ) if not use_legend: label = f"{signal_spec.label} [{signal_spec.units_display}]" ax.set_ylabel(label, color=signal_spec.color) ax.tick_params("y", colors=color) ax.set_ylim(signal_spec.min, signal_spec.max) outward_offset = 20 ax.xaxis.set_tick_params(direction="out") ax.yaxis.set_tick_params(direction="out") ax.spines["left"].set_position(("outward", outward_offset)) ax.spines["top"].set_color("none") # don't draw spine ax.spines["bottom"].set_position(("outward", outward_offset)) ax.spines["right"].set_position(("outward", outward_offset)) pos = {0: "left", 1: "right", 2: "right"}[i] ax.spines[pos].set_color(color) ax.xaxis.set_ticks_position("bottom") if use_legend: label = f"[{signal_spec.units_display}]" ax.set_ylabel(label) pylab.legend() bgr = a.get_bgr() plot_name = plot_name.replace("/", "") # output_filename = os.path.join('tmp', plot_name +'.png') # dtu.write_bgr_as_jpg(bgr, output_filename) t_inf = rospy.Time.from_sec(bag_in.get_end_time()) omsg = dru.d8_compressed_image_from_cv_image(bgr, timestamp=t_inf) otopic = prefix_out + "/" + plot_name bag_out.write(otopic, omsg, t=t_inf)
def query_logs(logs: Dict[str, PhysicalLog], query: QueryType, raise_if_no_matches=True) -> Dict[str, PhysicalLog]: """ query: a string or a list of strings Returns a dict str -> PhysicalLog. The query can also be a filename. """ if isinstance(query, list): res = {} for q in query: res.update(query_logs(logs, q, raise_if_no_matches=False)) if raise_if_no_matches and not res: msg = "Could not find any match for the queries:" for q in query: msg += f"\n- {q}" raise dtu.DTNoMatches(msg) return res else: dtu.check_isinstance(query, str) filters = {} filters.update(filters_slice) filters.update(dtu.filters0) aliases = {} aliases.update(logs) # adding aliases unless we are asking for everything if query != "*": # print('adding more (query = %s)' % query) for _, log in list(logs.items()): dtr = DTR.from_yaml(log.resources["bag"]) original_name = dtr.name # print ('alias: %s %s' % (original_name, dtr.name)) aliases[original_name] = log original_name = original_name.replace(".bag", "") aliases[original_name] = log result = dtu.fuzzy_match(query, aliases, filters=filters, raise_if_no_matches=raise_if_no_matches) # remove doubles after # XXX: this still has bugs present = defaultdict(set) for k, v in list(result.items()): present[id(v)].add(k) def choose(options): if len(options) == 1: return list(options)[0] else: options = sorted(options, key=len) return options[0] c = {} for k, v in list(result.items()): chosen = choose(present[id(v)]) if k == chosen: c[k] = v return c