def get_plot_phi_d(self,
ground_truth=None,
bgcolor=dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW):
figure_args = dict(facecolor=dtu.matplotlib_01_from_rgb(bgcolor))
a = 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 += "status = %s" % 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 += "\nest %s = %s" % (name, convert(estimate[name]))
if ground_truth is not None:
s += "\ntrue %s = %s" % (
name, convert(ground_truth_location[name]))
err = np.abs(ground_truth_location[name] - estimate[name])
s += '\nabs err = %s' % (convert(err))
cell = spec.resolution
percent = 100.0 / cell * err
s += '\nrel err = %.1f %% of cell' % (percent)
s += '\n true = green dot'
s += '\n'
s += "\nentropy = %.4f" % self.get_entropy()
s += "\nmax = %.4f" % self.belief.max()
s += "\nmin = %.4f" % self.belief.min()
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 = '%s [%s]' % (s2.label, s2.units_display)
pylab.ylabel(ylabel, color=s2.color)
xlabel = '%s [%s]' % (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,
tinfo,
segments,
dpi=120,
ground_truth=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=120,
bgcolor=dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW,
other_phi=None, other_d=None):
""" 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:
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 += "status = %s" % lane_filter.getStatus()
s += "\nphi = %.1f deg" % f_phi(phi)
s += "\nd = %.1f cm" % f_d(d)
s += "\nentropy = %.4f" % lane_filter.get_entropy()
s += "\nmax = %.4f" % belief.max()
s += "\nmin = %.4f" % belief.min()
if other_phi is not None:
s += '\n Other answers:'
for _phi, _d in zip(other_phi, other_d):
s += "\nphi = %.1f deg" % f_phi(_phi)
s += "\nd = %.1f cm" % f_d(_d)
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('phi: orientation (deg); cell = %.1f deg' % f_phi(delta_phi))
pylab.xlabel('d: distance from center line (cm); cell = %.1f cm' % f_d(delta_d))
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 = '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)