def grid_helper_plot(grid_helper, belief, truth=None, estimate=None):
a = dtu.CreateImageFromPylab(dpi=120)
with a as pylab:
grid_helper_plot_field(grid_helper, belief, pylab)
# pylab.axis('equal')
grid_helper_annotate_axes(grid_helper, pylab)
return a
def get_texture(smap: SegmentsMap, dpi: int) -> bytes:
figure_args = dict(figsize=(2, 2), facecolor="green")
a = dtu.CreateImageFromPylab(dpi=dpi, figure_args=figure_args)
frames = list(set(_.id_frame for _ in list(smap.points.values())))
id_frame = frames[0]
# print('frames: %s choose %s' % (frames, id_frame))
with a as pylab:
_plot_map_segments(smap, pylab, id_frame, plot_ref_segments=False)
pylab.axis("equal")
turn_all_axes_off(pylab)
pylab.tight_layout()
png = a.get_png()
return png
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)
def voting_kernel1():
resolution = 10.0
variables = {}
variables["x"] = dict(
min=100, max=500, description="x", resolution=resolution, units="cm", units_display="cm"
)
variables["y"] = dict(
min=100, max=500, description="y", resolution=resolution, units="cm", units_display="cm"
)
gh = GridHelper(variables)
votes = gh.create_new()
votes.fill(0)
points = []
estimated = []
estimated_weighted = []
F = 1
N = 25
errors_x = []
errors_x_w = []
for i in range(N):
dx = resolution / N
Dy = 70
Dx = 70
u = i / 5
v = i % 5
x = 125 + dx * i + Dx * u
y = 127 + Dy * v
p = dict(x=x, y=y)
points.append(p)
weight = 1
gh.add_vote(votes, p, weight=weight, F=F)
tmp = gh.create_new()
tmp.fill(0)
gh.add_vote(tmp, p, weight=weight, F=F)
assert_almost_equal(tmp.sum(), weight)
estimate = gh.get_max(tmp)
estimated.append(estimate)
estimate_weigthed = gh.get_max_weighted(tmp, F=F)
estimated_weighted.append(estimate_weigthed)
errors_x.append(p["x"] - estimate["x"])
errors_x_w.append(p["x"] - estimate_weigthed["x"])
errors_x = np.array(errors_x)
errors_x_w = np.array(errors_x_w)
dtu.logger.debug(f"errors_x: {errors_x}")
dtu.logger.debug(f"mean: {np.abs(errors_x).mean()}")
dtu.logger.debug(f"errors_x_w: {errors_x_w}")
dtu.logger.debug(f"mean: {np.abs(errors_x_w).mean()}")
assert errors_x.max() <= +resolution / 2
assert errors_x.min() >= -resolution / 2
assert np.abs(errors_x_w).max() <= resolution / 10
a = dtu.CreateImageFromPylab(dpi=1000)
with a as pylab:
grid_helper_plot_field(gh, votes, pylab)
pylab.axis("equal")
grid_helper_annotate_axes(gh, pylab)
for p in points:
grid_helper_mark_point(gh, pylab, p, color="blue", markersize=4)
for e in estimated:
grid_helper_mark_point(gh, pylab, e, color="red", markersize=3)
for e in estimated_weighted:
grid_helper_mark_point(gh, pylab, e, color="green", markersize=3)
b = dtu.CreateImageFromPylab(dpi=1000)
with b as pylab:
x = np.array([_["x"] for _ in points])
xe = np.array([_["x"] for _ in estimated])
xew = np.array([_["x"] for _ in estimated_weighted])
xe -= x
xew -= x
x = x * 0 # XXX?
pylab.plot(x, ".", label="x")
pylab.plot(xe, ".", label="x estimated")
pylab.plot(xew, ".", label="x estimated weighted")
pylab.legend()
od = dtu.get_output_dir_for_test()
fn = os.path.join(od, "voting_kernel1.jpg")
dtu.write_data_to_file(a.get_png(), fn)
fn = os.path.join(od, "errors.jpg")
dtu.write_data_to_file(b.get_png(), fn)