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()
def _do_plot(s1, d1, s2, d2): 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) with a as pylab: data_x = convert_unit(d1["data"], s1.units, s1.units_display) data_y = convert_unit(d2["data"], s2.units, s2.units_display) pylab.plot(data_x, data_y, ".") ylabel = f"{s2.label} [{s2.units_display}]" pylab.ylabel(ylabel, color=s2.color) xlabel = f"{s1.label} [{s1.units_display}]" pylab.xlabel(xlabel, color=s1.color) pylab.tick_params("y", colors=s2.color) pylab.ylim(s2.min, s2.max) pylab.xlim(s1.min, s1.max) bgr = a.get_bgr() return bgr
def plot_map_and_segments( sm: SegmentsMap, tinfo: TransformationsInfo, segments: List[SegMapSegment], dpi: int = 120, ground_truth: Optional[SE2value] = None, bgcolor=dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW, ): """Returns a BGR image""" figure_args = dict(facecolor=dtu.matplotlib_01_from_rgb(bgcolor)) a = dtu.CreateImageFromPylab(dpi=dpi, figure_args=figure_args) with a as pylab: _plot_map_segments(sm, pylab, FRAME_GLOBAL) _plot_detected_segments(tinfo, segments, pylab) # draw arrow L = 0.1 if ground_truth is not None: (x, y), _ = dtu.geo.translation_angle_from_SE2(ground_truth) x1, y1, _ = np.dot(ground_truth, [L, 0, 1]) pylab.plot(x, y, "co", markersize=12) pylab.plot([x, x1], [y, y1], "c-", linewidth=4) w1 = tinfo.transform_point(np.array([0, 0, 0]), frame1=FRAME_AXLE, frame2=FRAME_GLOBAL) w2 = tinfo.transform_point(np.array([L, 0, 0]), frame1=FRAME_AXLE, frame2=FRAME_GLOBAL) pylab.plot([w1[0], w2[0]], [w1[1], w2[1]], "m-") pylab.plot(w1[0], w1[1], "mo", markersize=6) pylab.axis("equal") return a.get_bgr()
def plot_phi_d_diagram_bgr( lane_filter, belief, phi, d, dpi: int = 120, bgcolor: dtu.BGRColor8 = dtu.ColorConstants.BGR_DUCKIETOWN_YELLOW, other_phi=None, other_d=None, ) -> dtu.NPImageBGR: """Returns a BGR image""" facecolor = dtu.matplotlib_01_from_rgb( dtu.rgb_color_from_bgr_color(bgcolor)) figure_args = dict(facecolor=facecolor) a = dtu.CreateImageFromPylab(dpi=dpi, figure_args=figure_args) with a as pylab: f_d = lambda x: 100 * x f_phi = np.rad2deg # Where are the lanes? lane_width = lane_filter.lanewidth d_max = lane_filter.d_max d_min = lane_filter.d_min phi_max = lane_filter.phi_max phi_min = lane_filter.phi_min delta_d = lane_filter.delta_d delta_phi = lane_filter.delta_phi # note transpose belief = belief.copy() zeros = belief == 0 belief[zeros] = np.nan belief_image = scale(belief, min_value=0) x = f_d(lane_filter.d_pcolor) y = f_phi(lane_filter.phi_pcolor) z = belief_image[:, :, 0] # just R component z = ma.masked_array(z, zeros) pylab.pcolor(x, y, np.ones(z.shape), cmap="Pastel1") pylab.pcolor(x, y, z, cmap="gray") if other_phi is not None: for _phi, _d in zip(other_phi, other_d): pylab.plot( f_d(_d), f_phi(_phi), "go", markersize=15, markeredgecolor="none", markeredgewidth=3, markerfacecolor="blue", ) pylab.plot( f_d(d), f_phi(phi), "go", markersize=20, markeredgecolor="magenta", markeredgewidth=3, markerfacecolor="none", ) pylab.plot( f_d(d), f_phi(phi), "o", markersize=2, markeredgecolor="none", markeredgewidth=0, markerfacecolor="magenta", ) W = f_d(lane_width / 2) width_white = f_d(lane_filter.linewidth_white) width_yellow = f_d(lane_filter.linewidth_yellow) pylab.plot([-W, -W], [f_phi(phi_min), f_phi(phi_max)], "k-") pylab.plot([-W - width_white, -W - width_white], [f_phi(phi_min), f_phi(phi_max)], "k-") pylab.plot([0, 0], [f_phi(phi_min), f_phi(phi_max)], "k-") pylab.plot([+W, +W], [f_phi(phi_min), f_phi(phi_max)], "y-") pylab.plot([+W + width_yellow, +W + width_yellow], [f_phi(phi_min), f_phi(phi_max)], "y-") s = "" s += f"status = {lane_filter.getStatus()}" s += f"\nphi = {f_phi(phi):.1f} deg" s += f"\nd = {f_d(d):.1f} cm" s += f"\nentropy = {lane_filter.get_entropy():.4f}" s += f"\nmax = {belief.max():.4f}" s += f"\nmin = {belief.min():.4f}" if other_phi is not None: s += "\n Other answers:" for _phi, _d in zip(other_phi, other_d): s += f"\nphi = {f_phi(_phi):.1f} deg" s += f"\nd = {f_d(_d):.1f} cm" y = f_phi(phi_max) - 10 args = dict(rotation=-90, color="white") annotate = True if annotate: pylab.annotate(s, xy=(0.7, 0.35), xycoords="figure fraction") pylab.annotate("in middle of right lane", xy=(0, y), **args) pylab.annotate("on right white tape", xy=(-W, y), **args) pylab.annotate("on left yellow tape", xy=(+W, y), **args) pylab.annotate("in other lane", xy=(+W * 1.3, y), **args) pylab.axis([f_d(d_min), f_d(d_max), f_phi(phi_min), f_phi(phi_max)]) pylab.ylabel( f"phi: orientation (deg); cell = {f_phi(delta_phi):.1f} deg") pylab.xlabel( f"d: distance from center line (cm); cell = {f_d(delta_d):.1f} cm") return a.get_bgr()
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)