def draw_boltzmann_actions(self, axis: Axes,
actions_value_fcn_dict: Dict[Tuple[int, int],
Dict[str, float]],
*args, **kwargs) -> None:
arrow_half_length = GridWorld.ARROW_LENGTH / 2.0
for state, actions_value_dict in actions_value_fcn_dict.items():
x, y = state
max_value = max(actions_value_dict.values())
max_action = max(actions_value_dict, key=actions_value_dict.get)
for action, value in actions_value_dict.items():
delx, dely = GridWorld.ACTION_DELTA_XY_DICT[action]
length = arrow_half_length * np.exp(value - max_value)
kwargs_copied = kwargs.copy()
if action == max_action:
kwargs_copied.update(color="r")
axis.arrow(
x,
y,
length * delx,
length * dely,
head_width=0.3 * length,
length_includes_head=True,
*args,
**kwargs_copied,
)
self._draw_grid_world(axis)
def arrows_transitions(
ax: Axes,
X: np.ndarray,
indices: Sequence[int],
weight=None,
):
"""
Plot arrows of transitions in data matrix.
Parameters
----------
ax
Axis object from matplotlib.
X
Data array, any representation wished (X, psi, phi, etc).
indices
Indices storing the transitions.
"""
step = 1
width = axis_to_data(ax, 0.001)
if X.shape[0] > 300:
step = 5
width = axis_to_data(ax, 0.0005)
if X.shape[0] > 500:
step = 30
width = axis_to_data(ax, 0.0001)
head_width = 10 * width
for ix, x in enumerate(X):
if ix % step != 0:
continue
X_step = X[indices[ix]] - x
# don't plot arrow of length 0
for itrans in range(X_step.shape[0]):
alphai = 1
widthi = width
head_widthi = head_width
if weight is not None:
alphai *= weight[ix, itrans]
widthi *= weight[ix, itrans]
if not np.any(X_step[itrans, :1]):
continue
ax.arrow(
x[0],
x[1],
X_step[itrans, 0],
X_step[itrans, 1],
length_includes_head=True,
width=widthi,
head_width=head_widthi,
alpha=alphai,
color='grey',
)
def plot_2d(self,
ax_2d: Axes,
point: array_like = (0, 0),
scalar: float = 1,
**kwargs) -> None:
"""
Plot a 2D vector.
The vector is plotted as an arrow.
Parameters
----------
ax_2d : Axes
Instance of :class:`~matplotlib.axes.Axes`.
point : array_like, optional
Position of the vector tail (default is origin).
scalar : {int, float}, optional
Value used to scale the vector (default 1).
kwargs : dict, optional
Additional keywords passed to :meth:`~matplotlib.axes.Axes.arrow`.
Examples
--------
.. plot::
:include-source:
>>> import matplotlib.pyplot as plt
>>> from skspatial.objects import Vector
>>> _, ax = plt.subplots()
>>> Vector([1, 1]).plot_2d(ax, point=(-3, 5), scalar=2, head_width=0.5)
>>> limits = ax.axis([-5, 5, 0, 10])
"""
x, y = point
dx, dy = scalar * self
ax_2d.arrow(x, y, dx, dy, **kwargs)
def draw_deterministic_actions(self, axis: Axes,
actions_value_fcn_dict: Dict[Tuple[int,
int],
Dict[str,
float]],
*args, **kwargs) -> None:
arrow_length = GridWorld.ARROW_LENGTH
arrow_half_length = arrow_length / 2.0
for state, actions_value_dict in actions_value_fcn_dict.items():
x, y = state
best_action = max(actions_value_dict, key=actions_value_dict.get)
delx, dely = GridWorld.ACTION_DELTA_XY_DICT[best_action]
axis.arrow(
x - arrow_half_length * delx,
y - arrow_half_length * dely,
arrow_length * delx,
arrow_length * dely,
head_width=0.1 * arrow_length,
length_includes_head=True,
*args,
**kwargs,
)
self._draw_grid_world(axis)
def render_sample_data(self,
sample_data_token: str,
with_anns: bool = True,
box_vis_level: BoxVisibility = BoxVisibility.ANY,
axes_limit: float = 40,
ax: Axes = None,
nsweeps: int = 1) -> None:
"""
Render sample data onto axis.
:param sample_data_token: Sample_data token.
:param with_anns: Whether to draw annotations.
:param box_vis_level: If sample_data is an image, this sets required visibility for boxes.
:param axes_limit: Axes limit for lidar and radar (measured in meters).
:param ax: Axes onto which to render.
:param nsweeps: Number of sweeps for lidar and radar.
"""
# Get sensor modality.
sd_record = self.nusc.get('sample_data', sample_data_token)
sensor_modality = sd_record['sensor_modality']
if sensor_modality == 'lidar':
# Get boxes in lidar frame.
_, boxes, _ = self.nusc.get_sample_data(sample_data_token, box_vis_level=box_vis_level)
# Get aggregated point cloud in lidar frame.
sample_rec = self.nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
ref_chan = 'LIDAR_TOP'
pc, times = LidarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps)
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists/axes_limit/np.sqrt(2))
ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=(c, c, c))
# Limit visible range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'radar':
# Get boxes in lidar frame.
sample_rec = self.nusc.get('sample', sd_record['sample_token'])
lidar_token = sample_rec['data']['LIDAR_TOP']
_, boxes, _ = self.nusc.get_sample_data(lidar_token, box_vis_level=box_vis_level)
# Get aggregated point cloud in lidar frame.
# The point cloud is transformed to the lidar frame for visualization purposes.
chan = sd_record['channel']
ref_chan = 'LIDAR_TOP'
pc, times = RadarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps)
# Transform radar velocities (x is front, y is left), as these are not transformed when loading the point
# cloud.
radar_cs_record = self.nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token'])
lidar_sd_record = self.nusc.get('sample_data', lidar_token)
lidar_cs_record = self.nusc.get('calibrated_sensor', lidar_sd_record['calibrated_sensor_token'])
velocities = pc.points[8:10, :] # Compensated velocity
velocities = np.vstack((velocities, np.zeros(pc.points.shape[1])))
velocities = np.dot(Quaternion(radar_cs_record['rotation']).rotation_matrix, velocities)
velocities = np.dot(Quaternion(lidar_cs_record['rotation']).rotation_matrix.T, velocities)
velocities[2, :] = np.zeros(pc.points.shape[1])
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists / axes_limit / np.sqrt(2))
sc = ax.scatter(points[0, :], points[1, :], c=colors, s=3)
# Show velocities.
points_vel = view_points(pc.points[:3, :] + velocities, np.eye(4), normalize=False)
max_delta = 10
deltas_vel = points_vel - points
deltas_vel = 3 * deltas_vel # Arbitrary scaling
deltas_vel = np.clip(deltas_vel, -max_delta, max_delta) # Arbitrary clipping
colors_rgba = sc.to_rgba(colors)
for i in range(points.shape[1]):
ax.arrow(points[0, i], points[1, i], deltas_vel[0, i], deltas_vel[1, i], color=colors_rgba[i])
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=(c, c, c))
# Limit visible range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'camera':
# Load boxes and image.
data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(sample_data_token,
box_vis_level=box_vis_level)
data = Image.open(data_path)
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 16))
# Show image.
ax.imshow(data)
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=camera_intrinsic, normalize=True, colors=(c, c, c))
# Limit visible range.
ax.set_xlim(0, data.size[0])
ax.set_ylim(data.size[1], 0)
else:
raise ValueError("Error: Unknown sensor modality!")
ax.axis('off')
ax.set_title(sd_record['channel'])
ax.set_aspect('equal')
def gauge(ax: Axes, bounds: Tuple[float, float], arrow: float, labels=('A', 'B', 'C'), colors='plasma', title: str = '',
bounds_check: bool = False):
"""
some sanity checks first
"""
n = len(labels)
if bounds_check and not (bounds[0] <= arrow <= bounds[1]):
raise ValueError('Arrow position out of range')
arrow = np.clip(arrow, *bounds)
"""
if colors is a string, we assume it's a matplotlib colormap
and we discretize in n discrete colors
"""
if isinstance(colors, str):
cmap = cm.get_cmap(colors, n)
cmap = cmap(np.arange(n))
colors = cmap[::, :].tolist()
if isinstance(colors, list):
if len(colors) == n:
colors = colors[::-1]
else:
raise Exception("\n\nnumber of colors {} not equal \
to number of categories{}\n".format(len(colors), n))
"""
begins the plotting
"""
ang_range, mid_points = _degree_range(n)
labels = labels[::-1]
"""
plots the sectors and the arcs
"""
patches = []
for ang, c in zip(ang_range, colors):
# sectors
patches.append(Wedge((0., 0.), .4, *ang, facecolor='w', lw=2))
# arcs
patches.append(Wedge((0., 0.), .4, *ang, width=0.10, facecolor=c, lw=2, alpha=1.0))
[ax.add_patch(p) for p in patches]
"""
set the autolabel_service
"""
for mid, lab in zip(mid_points, labels):
ax.text(0.35 * np.cos(np.radians(mid)), 0.35 * np.sin(np.radians(mid)), lab,
horizontalalignment='center', verticalalignment='center', fontsize=14,
fontweight='bold', rotation=_rot_text(mid))
"""
set the bottom banner and the title
"""
r = Rectangle((-0.4, -0.1), 0.8, 0.1, facecolor='w', lw=2)
ax.add_patch(r)
if "{}" in title:
title = title.format(arrow)
ax.text(0, -0.05, title, horizontalalignment='center',
verticalalignment='center', fontsize=22, fontweight='bold')
"""
plots the arrow now
"""
tile = 180 / n / 2
swing = 180 - 2 * tile
pos = tile + swing - (arrow - bounds[0]) / (bounds[1] - bounds[0]) * swing
ax.arrow(0, 0, 0.225 * np.cos(np.radians(pos)), 0.225 * np.sin(np.radians(pos)),
width=0.04, head_width=0.09, head_length=0.1, fc='k', ec='k')
ax.add_patch(Circle((0, 0), radius=0.02, facecolor='k'))
ax.add_patch(Circle((0, 0), radius=0.01, facecolor='w', zorder=11))
"""
removes frame and ticks, and makes axis equal and tight
"""
ax.set_frame_on(False)
ax.axes.set_xticks([])
ax.axes.set_yticks([])
ax.axis('equal')
