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