def test_synthetic(actual_map_name, template, robot_name, line_detector_name, image_prep_name, lane_filter_name, pose_or_location, outd): np.random.seed(42) db = get_easy_algo_db() actual_map = db.create_instance(FAMILY_SEGMAPS, actual_map_name) # first, load calibration for robot easy_algo_db = get_easy_algo_db() dtu.logger.debug('looking for localization template %r' % template) localization_template = easy_algo_db.create_instance( FAMILY_LOC_TEMPLATES, template) gp = get_ground_projection(robot_name) # GroundProjectionGeometry gpg = gp.get_ground_projection_geometry() if pose_or_location.shape == (3, 3): # SE(2) pose = pose_or_location location = localization_template.coords_from_pose(pose) else: location = pose_or_location pose = localization_template.pose_from_coords(location) simulation_data = simulate_image(actual_map, pose, gpg, blur_sigma=0.3) image = simulation_data.distorted_synthetic_bgr # anti_instagram_name='identity' # skip anti_instagram_name = 'baseline' all_details = False res, stats = run_pipeline(image, gp, line_detector_name=line_detector_name, image_prep_name=image_prep_name, lane_filter_name=lane_filter_name, anti_instagram_name=anti_instagram_name, all_details=all_details, ground_truth=pose, actual_map=actual_map) error = np.empty_like(location) for k in error.dtype.fields: error[k] = stats['estimate'][k] - location[k] stats['error'] = error res = dtu.resize_small_images(res) dtu.write_bgr_images_as_jpgs(res, outd, extra_string=outd.split('/')[-1]) return res, stats
def go(self): db = get_easy_algo_db() extra = self.options.get_extra() if len(extra) > 1: msg = 'Only one extra param accepted.' raise DTUserError(msg) if len(extra) == 1: query = extra[0] else: query = 'all' persons = db.query_and_instance('person', query) if self.options.roster: out_file = expand_all(self.options.roster) outd = os.path.dirname(out_file) roster = create_roster(persons, outd) write_data_to_file(roster, out_file) else: # instance ll tags = set() for p in persons.values(): tags.update(p.get_tags()) print(list(persons)) print('tags: %s' % list(tags)) table = table_people(persons) print(table)
def process_log(self, bag_in, prefix: str, bag_out, prefix_out: str, utils: ProcessorUtilsInterface): algo_db = get_easy_algo_db() line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR, self.line_detector) image_prep = algo_db.create_instance("image_prep", self.image_prep) vehicle = dbu.which_robot(bag_in) topic = f"/{vehicle}/camera_node/image/compressed" context = FakeContext() transform = None frame = 0 for compressed_img_msg in dbu.d8n_bag_read_with_progress(bag_in, topic): with context.phase("decoding"): try: image_cv = dtu.bgr_from_jpg(compressed_img_msg.data) except ValueError as e: msg = f"Could not decode image: {e}" dtu.raise_wrapped(ValueError, e, msg) segment_list = image_prep.process(context, image_cv, line_detector, transform) rendered = vs_fancy_display(image_prep.image_cv, segment_list) rendered = dtu.d8_image_zoom_linear(rendered, 2) log_name = "log_name" time = 12 rendered = dtu.add_duckietown_header(rendered, log_name, time, frame) out = dru.d8n_image_msg_from_cv_image(rendered, "bgr8", same_timestamp_as=compressed_img_msg) # Write to the bag bag_out.write("processed", out) # out = d8n_image_msg_from_cv_image(image_cv, "bgr8", same_timestamp_as=compressed_img_msg) bag_out.write("image", compressed_img_msg) frame += 1
def define_jobs_context(self, context): easy_algo_db = get_easy_algo_db() expect = self.options["expect"] write_to_db = self.options["write"] delete = not self.options["debug_no_delete"] if not expect in RTCheck.CHECK_RESULTS: msg = f"Invalid expect status {expect}; must be one of {RTCheck.CHECK_RESULTS}." raise dtu.DTUserError(msg) query = self.options["tests"] regression_tests = easy_algo_db.query("regression_test", query, raise_if_no_matches=True) for rt_name in regression_tests: rt = easy_algo_db.create_instance("regression_test", rt_name) easy_logs_db = self.get_easy_logs_db() c = context.child(rt_name) outd = os.path.join(self.options["output"], "regression_tests", rt_name) jobs_rt(c, rt_name, rt, easy_logs_db, outd, expect, write_data_to_db=write_to_db, delete=delete)
def create_visuals(robots, actual_map_name, out): db = get_easy_algo_db() actual_map = db.create_instance(FAMILY_SEGMAPS, actual_map_name) res = OrderedDict() res2 = OrderedDict() for i, robot_name in enumerate(sorted(robots)): dtu.logger.info('%d/%d: %s' % (i, len(robots), robot_name)) try: gp = GroundProjection(robot_name) except (NoCameraInfoAvailable, NoHomographyInfoAvailable) as e: dtu.logger.warning('skipping %r: %s' % (robot_name, e)) continue gpg = gp.get_ground_projection_geometry() pose = np.eye(3) simulated_data = \ simulate_image(actual_map, pose, gpg, blur_sigma=1) res[robot_name] = simulated_data.rectified_synthetic_bgr res2[robot_name] = simulated_data.distorted_synthetic_bgr if not res: msg = 'No images to draw.' dtu.logger.error(msg) else: output = os.path.join(out, 'distorted') dtu.write_bgr_images_as_jpgs(res2, output) output = os.path.join(out, 'rectified') dtu.write_bgr_images_as_jpgs(res, output)
def compose_maps(grid_spacing, tiles): db = get_easy_algo_db() tinfo = TransformationsInfo() FRAME_GLOBAL = 'global' partial = [] for tile in tiles: cell = tile['cell'] name = tile['name'] rotation = tile['rotation'] tile_map = db.create_instance(FAMILY_SEGMAPS, name) x = cell[0] * grid_spacing y = cell[1] * grid_spacing theta = np.deg2rad(rotation) g = SE2_from_xytheta([x,y, theta]) tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_TILE, g=g) tile_map2 = tinfo.transform_map_to_frame(tile_map, FRAME_GLOBAL) partial.append(tile_map2) result = SegmentsMap.merge(partial) return result
def process_one_processor(processor_name_or_spec, prefix_in, prefix_out, bag_filename, next_bag_filename, t0_absolute, t1_absolute, log): dtu.DuckietownConstants.show_timeit_benchmarks = True easy_algo_db = get_easy_algo_db() processor = easy_algo_db.create_instance(ProcessorInterface.FAMILY, processor_name_or_spec) dtu.logger.info('in: bag_filename: %s' % bag_filename) if not os.path.exists(bag_filename): msg = 'File does not exist: %s' % bag_filename raise ValueError(msg) dtu.logger.info('out: next_bag_filename: %s' % next_bag_filename) dtu.logger.info('t0_absolute: %s' % t0_absolute) dtu.logger.info('t1_absolute: %s' % t1_absolute) log = download_if_necessary(log) filename = get_local_bag_file(log) original_bag = rosbag.Bag(filename) bag_absolute_t0_ref = original_bag.get_start_time() original_bag.close() dtu.d8n_make_sure_dir_exists(next_bag_filename) out_bag = rosbag.Bag(next_bag_filename, 'w') bag0 = rosbag.Bag(bag_filename) t0_rel = t0_absolute - bag0.get_start_time() t1_rel = t1_absolute - bag0.get_start_time() in_bag = dtu.BagReadProxy(bag0, t0_rel, t1_rel, bag_absolute_t0_ref=bag_absolute_t0_ref) utils = ProcessorUtils(bag_out=out_bag, log=log) processor.process_log(in_bag, prefix_in, out_bag, prefix_out, utils) in_bag.close() # also copy the other messages in_bag1 = rosbag.Bag(bag_filename) for topic, msg, t in in_bag1.read_messages(raw=True): out_bag.write(topic, msg, t, raw=True) in_bag1.close() out_bag.close() # r = rosbag.Bag(next_bag_filename) # for topic, msg, t in r.read_messages(raw=True): # pass # r.close() # cmd = ['rosbag', 'fix', next_bag_filename, next_bag_filename] # dtu.system_cmd_result(cwd='.', cmd=cmd, raise_on_error=True, display_stdout=True, # display_stderr=True) # cmd = ['rosbag', 'reindex', next_bag_filename] # dtu.system_cmd_result(cwd='.', cmd=cmd, raise_on_error=True, display_stdout=True, # display_stderr=True) return next_bag_filename
def on_parameters_changed(self, _first_time, updated): if "verbose" in updated: self.info(f"Verbose is now {self.config.verbose!r}") if "line_detector" in updated: db = get_easy_algo_db() self.detector = db.create_instance("line_detector", self.config.line_detector)
def on_parameters_changed(self, _first_time, updated): if 'verbose' in updated: self.info('Verbose is now %r' % self.config.verbose) if 'line_detector' in updated: db = get_easy_algo_db() self.detector = db.create_instance('line_detector', self.config.line_detector)
def job_analyze(log: str, analyzer: str): easy_algo_db = get_easy_algo_db() analyzer_instance = easy_algo_db.create_instance("analyzer", analyzer) in_bag = rosbag.Bag(log) results = {} dtu.logger.info(f"Running {analyzer} on {log}") analyzer_instance.analyze_log(in_bag, results) in_bag.close() return results
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 job_analyze(log, analyzer): easy_algo_db = get_easy_algo_db() analyzer_instance = easy_algo_db.create_instance('analyzer', analyzer) in_bag = rosbag.Bag(log) results = OrderedDict() dtu.logger.info('Running %s on %s' % (analyzer, log)) analyzer_instance.analyze_log(in_bag, results) in_bag.close() return results
def job_merge(results, analyzer): """ results: log name -> results dict """ easy_algo_db = get_easy_algo_db() analyzer_instance = easy_algo_db.create_instance('analyzer', analyzer) results = list(results.values()) total = merge_n(analyzer_instance, results) return total
def display_map(id_map, out): dtu.logger.info('id_map == %s' % id_map) db = get_easy_algo_db() smap = db.create_instance(FAMILY_SEGMAPS, id_map) texture_png = get_texture(smap, dpi=600) fn = os.path.join(out, '%s-texture.png' % (id_map)) dtu.write_data_to_file(texture_png, fn) simdata = simulate_camera_view(smap) fn = os.path.join(out, '%s-view.jpg' % (id_map)) dtu.write_bgr_to_file_as_jpg(simdata.rectified_synthetic_bgr, fn) fn = os.path.join(out, '%s-segments.jpg' % (id_map)) dtu.write_bgr_to_file_as_jpg(simdata.rectified_segments_bgr, fn)
def get_map(self): if self._map is None: # Need to do this here and not in constructor # because otherwise there is a loop in EasyAlgo db = get_easy_algo_db() self._map = db.create_instance(FAMILY_SEGMAPS, self.tile_name) frames = set(_.id_frame for _ in self._map.points.values()) if frames != set([FRAME_GLOBAL]): msg = ('Expected that all points in the map %r are in the frame %r.' % (self.tile_name, FRAME_GLOBAL)) msg += ' These are the frames: %s.' % frames raise ValueError(msg) return self._map
def get_map(self) -> SegmentsMap: if self._map is None: # Need to do this here and not in constructor # because otherwise there is a loop in EasyAlgo db = get_easy_algo_db() self._map = db.create_instance(FAMILY_SEGMAPS, self.tile_name) frames = set(_.id_frame for _ in list(self._map.points.values())) if frames != {FRAME_GLOBAL}: msg = f"Expected that all points in the map {self.tile_name!r} are in the frame " \ f"{FRAME_GLOBAL!r}." msg += f" These are the frames: {frames}." raise ValueError(msg) return self._map
def go(self): out = 'out-maps' extra = self.options.get_extra() if len(extra) == 0: query = '*' else: query = extra db = get_easy_algo_db() maps = list(db.query_and_instance(FAMILY_SEGMAPS, query)) self.debug('maps: %s' % maps) for id_map in maps: display_map(id_map, out)
def go(self): out = "out/maps" extra = self.options.get_extra() if len(extra) == 0: query = "*" else: query = extra db = get_easy_algo_db() maps = list(db.query_and_instance(FAMILY_SEGMAPS, query)) self.debug(f"maps: {maps}") for id_map in maps: display_map(id_map, out)
def display_map(id_map: str, out: str): logger.info(f"id_map = {id_map}") db = get_easy_algo_db() smap = db.create_instance(FAMILY_SEGMAPS, id_map) texture_png = get_texture(smap, dpi=600) fn = os.path.join(out, id_map, f"{id_map}-texture.png") dtu.write_data_to_file(texture_png, fn) simdata = simulate_camera_view(smap, robot_name=dtu.DuckietownConstants.ROBOT_NAME_FOR_TESTS) fn = os.path.join(out, id_map, f"{id_map}-rectified_synthetic.jpg") dtu.write_bgr_to_file_as_jpg(simdata.rectified_synthetic_bgr, fn) fn = os.path.join(out, id_map, f"{id_map}-rectified_segments.jpg") dtu.write_bgr_to_file_as_jpg(simdata.rectified_segments_bgr, fn) fn = os.path.join(out, id_map, f"{id_map}-distorted_synthetic.jpg") dtu.write_bgr_to_file_as_jpg(simdata.distorted_synthetic_bgr, fn)
def call_summary(): db = get_easy_algo_db() s = format_db(db) dtu.logger.info(s) errors = [] for f in db.family_name2config.values(): if not f.valid: errors.append('Family %s: %s' % (f.family_name, f.error_if_invalid)) for i in f.instances.values(): if not i.valid: errors.append('Family %s / instance %r:\n\n%s' % (f.family_name, i.instance_name, dtu.indent(i.error_if_invalid, ' '))) if errors: msg = '\n' + '\n'.join(errors) raise Exception(msg)
def call_summary(): db = get_easy_algo_db() s = format_db(db) dtu.logger.info(s) errors = [] for f in list(db.family_name2config.values()): if not f.valid: errors.append(f"Family {f.family_name}: {f.error_if_invalid}") for i in list(f.instances.values()): if not i.valid: errors.append( f"Family {f.family_name} / instance {i.instance_name!r}:\n\n" f"{dtu.indent(i.error_if_invalid, ' ')}" ) if errors: msg = "\n" + "\n".join(errors) raise Exception(msg)
def go(self): extra = self.options.get_extra() db = get_easy_algo_db() if len(extra) == 0: query = dtu.get_current_robot_name() else: query = extra robots = db.query('robot', query, raise_if_no_matches=True) self.debug('robots: %s' % sorted(robots)) actual_map_name = 'DT17_scenario_four_way' out = self.options.output create_visuals(robots, actual_map_name, out) for robot_name in robots: _ok = try_simulated_localization(robot_name)
def compute_check_results(rt_name, rt, results_all): current = make_entry(rt_name, results_all) algo_db = get_easy_algo_db() entries_names = algo_db.query("rdbe", f"parameters:regression_test_name:{rt_name}") dtu.logger.info(f"entries: {list(entries_names)}") entries = [] for name in entries_names: e = algo_db.create_instance("rdbe", name) entries.append(e) rdb = ResultDB(current=current, entries=entries) res = [] for cwc in rt.get_checks(): for check in cwc.checks: r = check.check(rdb) assert isinstance(r, CheckResult) res.append(r) return res
def process_log(self, bag_in, bag_out): algo_db = get_easy_algo_db() line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR, self.line_detector) image_prep = algo_db.create_instance('image_prep', self.image_prep) vehicle = dtu.which_robot(bag_in) topic = '/%s/camera_node/image/compressed' % vehicle context = FakeContext() transform = None frame = 0 for compressed_img_msg in dtu.d8n_bag_read_with_progress( bag_in, topic): with context.phase('decoding'): try: image_cv = dtu.bgr_from_jpg(compressed_img_msg.data) except ValueError as e: msg = 'Could not decode image: %s' % e dtu.raise_wrapped(ValueError, e, msg) segment_list = image_prep.process(context, image_cv, line_detector, transform) rendered = vs_fancy_display(image_prep.image_cv, segment_list) rendered = dtu.d8_image_zoom_linear(rendered, 2) log_name = 'log_name' time = 12 rendered = dtu.add_duckietown_header(rendered, log_name, time, frame) out = dtu.d8n_image_msg_from_cv_image( rendered, "bgr8", same_timestamp_as=compressed_img_msg) # Write to the bag bag_out.write('processed', out) # out = d8n_image_msg_from_cv_image(image_cv, "bgr8", same_timestamp_as=compressed_img_msg) bag_out.write('image', compressed_img_msg) frame += 1
def create_visuals(robots: List[str], actual_map_name: str, out: str): db = get_easy_algo_db() actual_map = db.create_instance(FAMILY_SEGMAPS, actual_map_name) res = {} res2 = {} for i, robot_name in enumerate(sorted(robots)): logger.info(f"{i}/{len(robots)}: {robot_name}") rcg = get_robot_camera_geometry(robot_name) pose = np.eye(3) simulated_data = simulate_image(actual_map, pose, gpg=rcg.gpg, rectifier=rcg.rectifier, blur_sigma=1) res[robot_name] = simulated_data.rectified_synthetic_bgr res2[robot_name] = simulated_data.distorted_synthetic_bgr if not res: msg = "No images to draw." dtu.logger.error(msg) else: output = os.path.join(out, "distorted") dtu.write_bgr_images_as_jpgs(res2, output) output = os.path.join(out, "rectified") dtu.write_bgr_images_as_jpgs(res, output)
def define_jobs_context(self, context): easy_algo_db = get_easy_algo_db() expect = self.options.expect write_to_db = self.options.write delete = not self.options.debug_no_delete if not expect in RTCheck.CHECK_RESULTS: msg = 'Invalid expect status %s; must be one of %s.' % (expect, RTCheck.CHECK_RESULTS) raise dtu.DTUserError(msg) query = self.options.tests regression_tests = easy_algo_db.query('regression_test', query, raise_if_no_matches=True) for rt_name in regression_tests: rt = easy_algo_db.create_instance('regression_test', rt_name) easy_logs_db = self.get_easy_logs_db() c = context.child(rt_name) outd = os.path.join(self.options.output, 'regression_tests', rt_name) jobs_rt(c, rt_name, rt, easy_logs_db, outd, expect, write_data_to_db=write_to_db, delete=delete)
def coordinates(): easy_algo_db = get_easy_algo_db() localization_template = easy_algo_db.create_instance( FAMILY_LOC_TEMPLATES, template) localization_template._init_metrics() center = localization_template.center offset = localization_template.offset pose = dtu.geo.SE2_from_translation_angle([center[0], center[1] - offset], np.deg2rad(0)) location = localization_template.coords_from_pose(pose) assert_almost_equal(location['phi'], 0) assert_almost_equal(location['d'], 0) DX = 0.01 pose = dtu.geo.SE2_from_translation_angle( [center[0], center[1] - offset + DX], np.deg2rad(0)) location = localization_template.coords_from_pose(pose) assert_almost_equal(location['phi'], 0) assert_almost_equal(location['d'], DX) pose = dtu.geo.SE2_from_translation_angle( [center[0], center[1] - offset + DX], np.deg2rad(10)) location = localization_template.coords_from_pose(pose) assert_almost_equal(location['phi'], np.deg2rad(10)) assert_almost_equal(location['d'], DX) pose = dtu.geo.SE2_from_translation_angle([center[0] + offset, center[1]], np.deg2rad(90)) location = localization_template.coords_from_pose(pose) assert_almost_equal(location['phi'], np.deg2rad(0)) assert_almost_equal(location['d'], 0) a = 0 pose = dtu.geo.SE2_from_translation_angle( [center[0] + offset * np.cos(a), center[1] + offset * np.sin(a)], np.deg2rad(45)) location = localization_template.coords_from_pose(pose) assert_almost_equal(location['phi'], np.deg2rad(-45)) assert_almost_equal(location['d'], 0) a = np.deg2rad(-40) pose = dtu.geo.SE2_from_translation_angle( [center[0] + offset * np.cos(a), center[1] + offset * np.sin(a)], np.deg2rad(50)) location = localization_template.coords_from_pose(pose) # print('offset: %s' % offset) # print('pose: %s' % SE2.friendly(pose)) # print('location: %s' % phi_d_friendly(location)) # assert_almost_equal(location['phi'], np.deg2rad(0)) assert_almost_equal(location['d'], 0) a = np.deg2rad(-40) D = 0.01 pose = dtu.geo.SE2_from_translation_angle([ center[0] + (offset + D) * np.cos(a), center[1] + (offset + D) * np.sin(a) ], np.deg2rad(50)) location = localization_template.coords_from_pose(pose) # print('offset: %s' % offset) # print('pose: %s' % SE2.friendly(pose)) # print('location: %s' % phi_d_friendly(location)) # assert_almost_equal(location['phi'], np.deg2rad(0)) assert_almost_equal(location['d'], -D) pose = dtu.geo.SE2_from_translation_angle([+0.1095, 0], np.deg2rad(0)) check_coords(localization_template, pose) pose = dtu.geo.SE2_from_translation_angle([+0.1095, 0], np.deg2rad(15)) check_coords(localization_template, pose) # pose = dtu.geo.SE2_from_translation_angle([0.15, -0.1], np.deg2rad(5)) check_coords(localization_template, pose)
def run_pipeline(image, all_details=False, ground_truth=None, actual_map=None): """ Image: numpy (H,W,3) == BGR Returns a dictionary, res with the following fields: res['input_image'] ground_truth = pose """ print('backend: %s' % matplotlib.get_backend()) print('fname: %s' % matplotlib.matplotlib_fname()) quick = False dtu.check_isinstance(image, np.ndarray) res = OrderedDict() stats = OrderedDict() vehicle_name = dtu.get_current_robot_name() res['Raw input image'] = image gp = get_ground_projection(vehicle_name) gpg = gp.get_ground_projection_geometry line_detector = LineDetector() lane_filter = LaneFilterHistogram(None) print("pass lane_filter") ai = AntiInstagram() pts = ProcessingTimingStats() pts.reset() pts.received_message() pts.decided_to_process() with pts.phase('calculate AI transform'): [lower, upper] = ai.calculate_color_balance_thresholds(image) with pts.phase('apply AI transform'): transformed = ai.apply_color_balance(lower, upper, image) with pts.phase('edge detection'): # note: do not apply transform twice! segment_list2 = image_prep.process(pts, image, line_detector, transform=ai.apply_color_balance) if all_details: res['resized and corrected'] = image_prep.image_corrected dtu.logger.debug('segment_list2: %s' % 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'] = gpg.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(gpg, segment_list2) # Project to ground with pts.phase('find_ground_coordinates'): sg = find_ground_coordinates(gpg, segment_list2_rect) lane_filter.initialize() if all_details: res['prior'] = lane_filter.get_plot_phi_d() with pts.phase('lane filter update'): print(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('Using default template %r for visualization' % template_name) 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 = gpg.rectify(image) rectified = ai.apply_color_balance(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 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 get_scuderia_contents(): db = get_easy_algo_db() robots = db.query_and_instance('robot', 'all') return robots