def name_from_spec(x): 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, instance_name_or_spec): dtu.check_isinstance(instance_name_or_spec, (dict, str)) family = self.get_family(family_name) if not family.valid: msg = ('Cannot instantiate %r because its family %r is invalid.' % (instance_name_or_spec, family_name)) 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 = ('Cannot instantiate %r because it is invalid:\n%s' % (instance_name, 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 = ('I expected that %r would be a %s but it is a %s.' % (instance_name, interface.__name__, type(res).__name__)) raise dtu.DTConfigException(msg) return res
def __init__(self, logs=None): # ordereddict str -> PhysicalLog if logs is None: logs = load_all_logs() else: check_isinstance(logs, OrderedDict) self.logs = logs
def logs_from_yaml(data): dtu.check_isinstance(data, dict) res = OrderedDict() for k, d in data.items(): res[k] = physical_log_from_yaml(d) 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 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 = 'Could not found any value for topic %r.' % topic_fqn raise ValueError(msg) return topic2messages
def _parse_inline_spec(x): dtu.check_isinstance(x, dict) if len(x) != 1: msg = 'Invalid spec: length is %d' % 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, NoneType)) if parameters is None: parameters = {} res = SpecValues(constructor=constructor, parameters=parameters, description=description) return key, res
def yaml_representation_of_phy_logs(logs): dtu.check_isinstance(logs, dict) res = OrderedDict() for k, l in logs.items(): res[k] = yaml_from_physical_log(l) s = yaml_dump_pretty(res) return s
def __str__(self): n = len(self.entries) s = 'ResultsDB with %d entries' % n 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), '', ' %2d of %d: ' % (i + 1, n)) return s
def __init__(self, variables, precision='float32'): self.precision = precision dtu.check_isinstance(variables, OrderedDict) if len(variables) != 2: msg = 'I can only deal with 2 variables, obtained %s' % self._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] = self._specs[a]._replace(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(range(H), 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): dtu.check_isinstance(robot_name, str) # Get the config file for the homography of the robot filename = get_homography_info_config_file(robot_name, strict=False) data = dtu.yaml_load_file(filename) # Extract the homography from the YAML file homography = homography_from_yaml(data) return homography
def get_homography_for_robot(robot_name): 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, True) # True means "plain" # convert the YAML homography = homography_from_yaml(data) return homography
def get_homography_for_robot(robot_name): dtu.check_isinstance(robot_name, str) # find the file if robot_name == dtu.DuckietownConstants.ROBOT_NAME_FOR_TESTS: data = dtu.yaml_load(homography_default) else: fn = get_homography_info_config_file(robot_name) # load the YAML data = dtu.yaml_load_file(fn) # convert the YAML homography = homography_from_yaml(data) #check_homography_sane_for_DB17(homography) return homography
def validate(self): for k, p in 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] = p._replace(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 = 'Invalid point %r' % p 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 = 'This is a degenerate segment (points: %s %s) ' % (w1, w2) msg += 'names: %s' % 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 = 'Invalid point %r' % p raise ValueError(msg)
def query(self, query, raise_if_no_matches=True): """ query: a string Returns an OrderedDict str -> PhysicalLog. """ check_isinstance(query, str) filters = OrderedDict() filters.update(filters_slice) filters.update(filters0) result = fuzzy_match(query, self.logs, filters=filters, raise_if_no_matches=raise_if_no_matches) return result
def download_if_necessary(log): """ 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) log.resources[resource_name]['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 += '\n branch: %s' % branch msg += '\n date: %s' % date msg += '\n commit: %s' % commit raise DataNotFound(msg) if len(possible) > 1: n = len(possible) msg = 'Found %d matches for this query.' % n msg += '\n branch: %s' % branch msg += '\n date: %s' % date msg += '\n commit: %s' % commit msg += '\nThese are the matches:' for i, p in enumerate(possible): dtu.check_isinstance(p, ResultDBEntry) msg += '\n' + dtu.indent(str(p), ' %2d of %d: ' % (i + 1, n)) raise AmbiguousQuery(msg) return possible[0]
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 = 'Could not found any value for topic %r.' % topic_fqn 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 = '%s [%s]' % (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 = '[%s]' % (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()) # @UndefinedVariable omsg = dtu.d8_compressed_image_from_cv_image(bgr, timestamp=t_inf) otopic = prefix_out + '/' + plot_name bag_out.write(otopic, omsg, t=t_inf)
def __init__(self, current, entries): for e in entries: dtu.check_isinstance(e, ResultDBEntry) self.entries = entries self.current = current dtu.check_isinstance(current, ResultDBEntry)
def query_logs(logs, query, raise_if_no_matches=True): """ query: a string or a list of strings Returns an OrderedDict str -> PhysicalLog. The query can also be a filename. """ if isinstance(query, list): res = OrderedDict() 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 += '\n- %s' % q raise dtu.DTNoMatches(msg) return res else: dtu.check_isinstance(query, str) filters = OrderedDict() filters.update(filters_slice) filters.update(dtu.filters0) aliases = OrderedDict() aliases.update(logs) # adding aliases unless we are asking for everything if query != '*': # print('adding more (query = %s)' % query) for _, log in 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 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 = OrderedDict() for k, v in result.items(): chosen = choose(present[id(v)]) if k == chosen: c[k] = v return c
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 go(self): robot_name = dtu.get_current_robot_name() output = self.options.output if output is None: output = 'out-calibrate-extrinsics' # + dtu.get_md5(self.options.image)[:6] self.info('No --output given, using %s' % output) if self.options.input is None: print("{}\nCalibrating using the ROS image stream...\n".format( "*" * 20)) import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join('/', robot_name, 'camera_node/image/compressed') print('Topic to listen to is: %s' % topic_name) print('Let\'s wait for an image. Say cheese!') # Dummy for getting a ROS message rospy.init_node('calibrate_extrinsics') img_msg = None try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) print('Image captured!') except rospy.ROSException as e: print( '\n\n\nDidn\'t get any message!: %s\n MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!\n\n\n' % (e, )) bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg)) self.info('Picture taken: %s ' % str(bgr.shape)) else: self.info('Loading input image %s' % self.options.input) bgr = dtu.bgr_from_jpg_fn(self.options.input) if bgr.shape[1] != 640: interpolation = cv2.INTER_CUBIC bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation) self.info('Resized to: %s ' % str(bgr.shape)) # Disable the old calibration file disable_old_homography(robot_name) camera_info = get_camera_info_for_robot(robot_name) homography_dummy = get_homography_default() gpg = GroundProjectionGeometry(camera_info, homography_dummy) res = OrderedDict() try: bgr_rectified = gpg.rectify(bgr, interpolation=cv2.INTER_CUBIC) res['bgr'] = bgr res['bgr_rectified'] = bgr_rectified _new_matrix, res['rectified_full_ratio_auto'] = gpg.rectify_full( bgr, ratio=1.65) result = estimate_homography(bgr_rectified) dtu.check_isinstance(result, HomographyEstimationResult) if result.bgr_detected_refined is not None: res['bgr_detected_refined'] = result.bgr_detected_refined if not result.success: raise Exception(result.error) save_homography(result.H, robot_name) msg = ''' To check that this worked well, place the robot on the road, and run: rosrun complete_image_pipeline single_image Look at the produced jpgs. ''' self.info(msg) finally: dtu.write_bgr_images_as_jpgs(res, output)