def plot_sfm_data_3d(sfm_data: GtsfmData,
ax: Axes,
max_plot_radius: float = 50) -> None:
"""Plot the camera poses and landmarks in 3D matplotlib plot.
Args:
sfm_data: SfmData object with camera and tracks.
ax: axis to plot on.
max_plot_radius: maximum distance threshold away from any camera for which a point
will be plotted
"""
camera_poses = [
sfm_data.get_camera(i).pose()
for i in sfm_data.get_valid_camera_indices()
]
plot_poses_3d(camera_poses, ax)
num_tracks = sfm_data.number_tracks()
# Restrict 3d points to some radius of camera poses
points_3d = np.array(
[list(sfm_data.get_track(j).point3()) for j in range(num_tracks)])
nearby_points_3d = comp_utils.get_points_within_radius_of_cameras(
camera_poses, points_3d, max_plot_radius)
# plot 3D points
for landmark in nearby_points_3d:
ax.plot(landmark[0], landmark[1], landmark[2], "g.", markersize=1)
def show_mask(self, ax: axes.Axes, title="", fontfamily="times new roman", fontsize=15, scatter=20):
phi = np.linspace(0, 2 * np.pi, 360 * 4)
circle = ax.fill(self._aperture * np.cos(phi), self._aperture * np.sin(phi), color="lightblue", zorder=0)
elements = ax.scatter(x=self._locations[0], y=self._locations[1], s=scatter, c="gray", zorder=2)
if title == "":
title = "Total number: " + str(self.get_total_number())
# loop line
for radius in self._radius:
ax.plot(radius * np.cos(phi), radius * np.sin(phi), color="orange", zorder=1)
ax.set_aspect("equal", 'box')
ax.set_axis_off()
ax.grid(True)
ax.set_title(title, fontsize=fontsize, fontfamily=fontfamily)
return ax
def plot_poses_3d(wTi_list: List[Optional[Pose3]],
ax: Axes,
center_marker_color: str = "k",
label_name: Optional[str] = None) -> None:
"""Plot poses in 3D as dots for centers and lines denoting the orthonormal
coordinate system for each camera.
Color convention: R -> x axis, G -> y axis, B -> z axis.
Args:
wTi_list: list of poses to plot.
ax: axis to plot on.
center_marker_color (optional): color for camera center marker. Defaults to "k".
name:
"""
spec = "{}.".format(center_marker_color)
is_label_added = False
for wTi in wTi_list:
if wTi is None:
continue
if is_label_added:
# for the rest of iterations, set label to None (otherwise would be duplicated in legend)
label_name = None
x, y, z = wTi.translation().squeeze()
ax.plot(x, y, z, spec, markersize=10, label=label_name)
is_label_added = True
R = wTi.rotation().matrix()
# getting the direction of the coordinate system (x, y, z axes)
default_axis_length = 0.5
v1 = R[:, 0] * default_axis_length
v2 = R[:, 1] * default_axis_length
v3 = R[:, 2] * default_axis_length
ax.plot3D([x, x + v1[0]], [y, y + v1[1]], [z, z + v1[2]], c="r")
ax.plot3D([x, x + v2[0]], [y, y + v2[1]], [z, z + v2[2]], c="g")
ax.plot3D([x, x + v3[0]], [y, y + v3[1]], [z, z + v3[2]], c="b")
def draw_coordinate_system_matplotlib(
coordinate_system: LocalCoordinateSystem,
axes: Axes,
color: Any = None,
label: str = None,
time_idx: int = None,
scale_vectors: Union[float, List, np.ndarray] = None,
show_origin: bool = True,
show_vectors: bool = True,
):
"""Draw a coordinate system in a matplotlib 3d plot.
Parameters
----------
coordinate_system :
Coordinate system
axes :
Target matplotlib axes object
color :
Valid matplotlib color selection. The origin of the coordinate system
will be marked with this color.
label :
Name that appears in the legend. Only viable if a color
was specified.
time_idx :
Selects time dependent data by index if the coordinate system has
a time dependency.
scale_vectors :
A scaling factor or array to adjust the vector length
show_origin :
If `True`, the origin of the coordinate system will be highlighted in the
color passed as another parameter
show_vectors :
If `True`, the the coordinate axes of the coordinate system are visualized
"""
if not (show_vectors or show_origin):
return
if "time" in coordinate_system.dataset.coords:
if time_idx is None:
time_idx = 0
if isinstance(time_idx, int):
dsx = coordinate_system.dataset.isel(time=time_idx)
else:
dsx = coordinate_system.dataset.sel(time=time_idx).isel(time=0)
else:
dsx = coordinate_system.dataset
p_0 = dsx.coordinates
if show_vectors:
if scale_vectors is None:
tips = dsx.orientation
else:
if not isinstance(scale_vectors, np.ndarray):
if isinstance(scale_vectors, List):
scale_vectors = np.array(scale_vectors)
else:
scale_vectors = np.array([scale_vectors for _ in range(3)])
scale_mat = np.eye(3, 3)
for i in range(3):
scale_mat[i, i] = scale_vectors[i]
tips = np.matmul(scale_mat, dsx.orientation.data)
p_x = p_0 + tips[:, 0]
p_y = p_0 + tips[:, 1]
p_z = p_0 + tips[:, 2]
axes.plot([p_0[0], p_x[0]], [p_0[1], p_x[1]], [p_0[2], p_x[2]], "r")
axes.plot([p_0[0], p_y[0]], [p_0[1], p_y[1]], [p_0[2], p_y[2]], "g")
axes.plot([p_0[0], p_z[0]], [p_0[1], p_z[1]], [p_0[2], p_z[2]], "b")
if color is not None:
if show_origin:
axes.plot([p_0[0]], [p_0[1]], [p_0[2]],
"o",
color=color,
label=label)
elif label is not None:
raise Exception("Labels can only be assigned if a color was specified")
def plot_spatial_data_matplotlib(
data: geo.SpatialData,
axes: Axes = None,
color: Union[int, Tuple[int, int, int], Tuple[float, float, float]] = None,
label: str = None,
show_wireframe: bool = True,
) -> Axes:
"""Visualize a `weldx.geometry.SpatialData` instance.
Parameters
----------
data :
The data that should be visualized
axes :
The target `matplotlib.axes.Axes` object of the plot. If 'None' is passed, a
new figure will be created
color :
A 24 bit integer, a triplet of integers with a value range of 0-255
or a triplet of floats with a value range of 0.0-1.0 that represent an RGB
color
label :
Label of the plotted geometry
show_wireframe :
If `True`, the mesh is plotted as wireframe. Otherwise only the raster
points are visualized. Currently, the wireframe can't be visualized if a
`weldx.geometry.VariableProfile` is used.
Returns
-------
matplotlib.axes.Axes :
The `matplotlib.axes.Axes` instance that was used for the plot.
"""
if axes is None:
_, axes = new_3d_figure_and_axes()
if not isinstance(data, geo.SpatialData):
data = geo.SpatialData(data)
if color is None:
color = (0.0, 0.0, 0.0)
else:
color = color_to_rgb_normalized(color)
coordinates = data.coordinates.data
triangles = data.triangles
# if data is time dependent or has other extra dimensions, just take the first value
while coordinates.ndim > 2:
coordinates = coordinates[0]
axes.scatter(
coordinates[:, 0],
coordinates[:, 1],
coordinates[:, 2],
marker=".",
color=color,
label=label,
zorder=2,
)
if triangles is not None and show_wireframe:
for triangle in triangles:
triangle_data = coordinates[[*triangle, triangle[0]], :]
axes.plot(
triangle_data[:, 0],
triangle_data[:, 1],
triangle_data[:, 2],
color=color,
zorder=1,
)
return axes
def plot_local_coordinate_system_matplotlib(
lcs: LocalCoordinateSystem,
axes: Axes = None,
color: Any = None,
label: str = None,
time: types_timeindex = None,
time_ref: pd.Timestamp = None,
time_index: int = None,
scale_vectors: Union[float, List, np.ndarray] = None,
show_origin: bool = True,
show_trace: bool = True,
show_vectors: bool = True,
) -> Axes:
"""Visualize a `weldx.transformations.LocalCoordinateSystem` using matplotlib.
Parameters
----------
lcs :
The coordinate system that should be visualized
axes :
The target matplotlib axes. If `None` is provided, a new one will be created
color :
An arbitrary color. The data type must be compatible with matplotlib.
label :
Name of the coordinate system
time :
The time steps that should be plotted
time_ref :
A reference timestamp that can be provided if the ``time`` parameter is a
`pandas.TimedeltaIndex`
time_index :
Index of a specific time step that should be plotted
scale_vectors :
A scaling factor or array to adjust the vector length
show_origin :
If `True`, the origin of the coordinate system will be highlighted in the
color passed as another parameter
show_trace :
If `True`, the trace of a time dependent coordinate system will be visualized in
the color passed as another parameter
show_vectors :
If `True`, the the coordinate axes of the coordinate system are visualized
Returns
-------
matplotlib.axes.Axes :
The axes object that was used as canvas for the plot.
"""
if axes is None:
_, axes = plt.subplots(subplot_kw={
"projection": "3d",
"proj_type": "ortho"
})
if lcs.is_time_dependent and time is not None:
lcs = lcs.interp_time(time, time_ref)
if lcs.is_time_dependent and time_index is None:
for i, _ in enumerate(lcs.time):
draw_coordinate_system_matplotlib(
lcs,
axes,
color=color,
label=label,
time_idx=i,
scale_vectors=scale_vectors,
show_origin=show_origin,
show_vectors=show_vectors,
)
label = None
else:
draw_coordinate_system_matplotlib(
lcs,
axes,
color=color,
label=label,
time_idx=time_index,
scale_vectors=scale_vectors,
show_origin=show_origin,
show_vectors=show_vectors,
)
if show_trace and lcs.coordinates.values.ndim > 1:
coords = lcs.coordinates.values
if color is None:
color = "k"
axes.plot(coords[:, 0], coords[:, 1], coords[:, 2], ":", color=color)
return axes
def plot_spatial_data_matplotlib(
data: Union[np.ndarray, geo.SpatialData],
axes: Axes = None,
color: Union[int, tuple[int, int, int], tuple[float, float, float]] = None,
label: str = None,
limits: types_limits = None,
show_wireframe: bool = True,
) -> Axes:
"""Visualize a `weldx.geometry.SpatialData` instance.
Parameters
----------
data :
The data that should be visualized
axes :
The target `matplotlib.axes.Axes` object of the plot. If 'None' is passed, a
new figure will be created
color :
A 24 bit integer, a triplet of integers with a value range of 0-255
or a triplet of floats with a value range of 0.0-1.0 that represent an RGB
color
label :
Label of the plotted geometry
limits :
Each tuple marks lower and upper boundary of the x, y and z axis. If only a
single tuple is passed, the boundaries are used for all axis. If `None`
is provided, the axis are adjusted to be of equal length.
show_wireframe :
If `True`, the mesh is plotted as wireframe. Otherwise only the raster
points are visualized. Currently, the wireframe can't be visualized if a
`weldx.geometry.VariableProfile` is used.
Returns
-------
matplotlib.axes.Axes :
The `matplotlib.axes.Axes` instance that was used for the plot.
"""
if axes is None:
_, axes = new_3d_figure_and_axes()
if not isinstance(data, geo.SpatialData):
data = geo.SpatialData(data)
if color is None:
color = (0.0, 0.0, 0.0)
else:
color = color_to_rgb_normalized(color)
coordinates = data.coordinates.data
if isinstance(coordinates, Q_):
coordinates = coordinates.to(_DEFAULT_LEN_UNIT).m
coordinates = coordinates.reshape(-1, 3)
triangles = data.triangles
# if data is time dependent or has other extra dimensions, just take the first value
while coordinates.ndim > 2:
coordinates = coordinates[0]
axes.scatter(
coordinates[:, 0],
coordinates[:, 1],
coordinates[:, 2],
marker=".",
color=color,
label=label,
zorder=2,
)
if triangles is not None and show_wireframe:
for triangle in triangles:
triangle_data = coordinates[[*triangle, triangle[0]], :]
axes.plot(
triangle_data[:, 0],
triangle_data[:, 1],
triangle_data[:, 2],
color=color,
zorder=1,
)
_set_limits_matplotlib(axes, limits)
return axes
