From d1780601cd44498f2acb0b3e8add64a2d95584c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Apr 2025 09:50:41 -0400 Subject: [PATCH] SVG graphs --- python/gtsam/examples/EKF_SLAM.ipynb | 9 +- python/gtsam/examples/gtsam_plotly.py | 142 +++++++++++++++++--------- python/gtsam/examples/simulation.py | 2 +- 3 files changed, 99 insertions(+), 54 deletions(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 265005710..48a262f5e 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -108,17 +108,12 @@ "import numpy as np\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", - "import time # To slow down graphviz display if needed\n", "\n", "import gtsam\n", "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", "\n", - "# Imports for new modules\n", "import simulation\n", - "from gtsam_plotly import SlamFrameData, create_slam_animation\n", - "\n", - "# Imports for graph visualization\n", - "import graphviz" + "from gtsam_plotly import SlamFrameData, create_slam_animation" ] }, { @@ -143,7 +138,7 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 20 # Reduced steps for faster animation generation\n", + "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", "DT = 1.0 # Time step duration (simplified)\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index dbbae8f46..a0d6e0ed3 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -164,17 +164,30 @@ def create_landmark_ellipse_shape( ) -def dot_string_to_base64_png( +def dot_string_to_base64_svg( dot_string: Optional[str], engine: str = "neato" ) -> Optional[str]: - """Renders a DOT string to a base64 encoded PNG using graphviz.""" + """Renders a DOT string to a base64 encoded SVG using graphviz.""" if not dot_string: return None - source = graphviz.Source(dot_string, engine="neato") - # Use pipe() to get bytes directly without saving a file - png_bytes = source.pipe(format="png") - encoded_string = base64.b64encode(png_bytes).decode() - return f"data:image/png;base64,{encoded_string}" + try: + # Set DPI for potentially better default sizing, though less critical for SVG + # graph_attr = {'dpi': '150'} # You can experiment with this + source = graphviz.Source(dot_string, engine=engine) # , graph_attr=graph_attr) + # Use pipe() to get bytes directly without saving a file, format="svg" + svg_bytes = source.pipe(format="svg") + # Encode bytes to base64 string, decode result to UTF-8 string + encoded_string = base64.b64encode(svg_bytes).decode("utf-8") + # Return data URI for SVG + return f"data:image/svg+xml;base64,{encoded_string}" + except graphviz.backend.execute.CalledProcessError as e: + print(f"Graphviz rendering error ({engine}): {e}") + # Optionally return a placeholder or raise + return None + except Exception as e: + print(f"Unexpected error during Graphviz SVG generation: {e}") + return None + # --- Frame Content Generation --- def generate_frame_content( @@ -183,7 +196,7 @@ def generate_frame_content( L: Callable[[int], int], max_landmark_index: int, ellipse_scale: float = 2.0, - graphviz_engine: str = "neato", # Added engine parameter + graphviz_engine: str = "neato", verbose: bool = False, ) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: """Generates dynamic traces, shapes, and layout image for a single frame.""" @@ -191,11 +204,11 @@ def generate_frame_content( step_results = frame_data.results step_marginals = frame_data.marginals - frame_traces: List[go.Scatter] = [] + frame_dynamic_traces: List[go.Scatter] = [] # Renamed for clarity frame_shapes: List[Dict[str, Any]] = [] layout_image: Optional[Dict[str, Any]] = None - # 1. Estimated Path (Unchanged) + # 1. Estimated Path est_path_x = [ step_results.atPose2(X(i)).x() for i in range(k + 1) @@ -206,9 +219,9 @@ def generate_frame_content( for i in range(k + 1) if step_results.exists(X(i)) ] - frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) + frame_dynamic_traces.append(create_est_path_trace(est_path_x, est_path_y)) - # 2. Estimated Landmarks (Unchanged) + # 2. Estimated Landmarks est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) @@ -219,9 +232,9 @@ def generate_frame_content( landmark_keys.append(lm_key) lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: - frame_traces.append(lm_trace) + frame_dynamic_traces.append(lm_trace) - # 3. Covariance Ellipses (Unchanged) + # 3. Covariance Ellipses if step_marginals: pose_key = X(k) if step_results.exists(pose_key): @@ -229,9 +242,13 @@ def generate_frame_content( cov = step_marginals.marginalCovariance(pose_key) mean = step_results.atPose2(pose_key).translation() frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) - except Exception as e: + except RuntimeError as e: # More specific exception type if verbose: print(f"Warn: Pose {k} cov err @ step {k}: {e}") + except Exception as e: + if verbose: + print(f"Warn: Pose {k} cov OTHER err @ step {k}: {e}") + for lm_key in landmark_keys: try: cov = step_marginals.marginalCovariance(lm_key) @@ -239,15 +256,20 @@ def generate_frame_content( frame_shapes.append( create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) - except Exception as e: + except RuntimeError as e: # More specific exception type if verbose: print( f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" ) + except Exception as e: + if verbose: + print( + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov OTHER err @ step {k}: {e}" + ) - # 4. Graph Image for Layout (MODIFIED) - # Use the new function with the dot string from frame_data - img_src = dot_string_to_base64_png( + # 4. Graph Image for Layout (MODIFIED TO USE SVG) + # Use the SVG function with the dot string from frame_data + img_src = dot_string_to_base64_svg( # Changed function call frame_data.graph_dot_string, engine=graphviz_engine ) if img_src: @@ -262,10 +284,11 @@ def generate_frame_content( xanchor="left", yanchor="top", layer="below", - sizing="contain", + sizing="contain", # Contain should work well with SVG ) - return frame_traces, frame_shapes, layout_image + # Return only the things that CHANGE frame-to-frame + return frame_dynamic_traces, frame_shapes, layout_image # --- Figure Configuration --- @@ -374,6 +397,9 @@ def configure_figure_layout( # --- Main Animation Orchestrator --- +# --- Main Animation Orchestrator (MODIFIED TO FIX GT DISPLAY) --- + + def create_slam_animation( history: List[SlamFrameData], X: Callable[[int], int], @@ -383,7 +409,7 @@ def create_slam_animation( poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, - graphviz_engine: str = "neato", # Pass engine choice + graphviz_engine: str = "neato", verbose_cov_errors: bool = False, ) -> go.Figure: """Creates a side-by-side Plotly SLAM animation using a history of dataclasses.""" @@ -393,31 +419,63 @@ def create_slam_animation( num_steps = history[-1].step_index fig = go.Figure() - # 1. Add static GT traces (Unchanged) + # 1. Create static GT traces ONCE + gt_traces = [] gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) if gt_lm_trace: - fig.add_trace(gt_lm_trace) + gt_traces.append(gt_lm_trace) gt_path_trace = create_gt_path_trace(poses_gt) if gt_path_trace: - fig.add_trace(gt_path_trace) + gt_traces.append(gt_path_trace) - # 2. Generate frames (MODIFIED: pass graphviz_engine) + # 2. Generate content for the initial frame (k=0) to set up the figure + initial_frame_data = next((item for item in history if item.step_index == 0), None) + if initial_frame_data is None: + raise ValueError("History must contain data for step 0.") + + ( + initial_dynamic_traces, + initial_shapes, + initial_image, + ) = generate_frame_content( + initial_frame_data, + X, + L, + max_landmark_index, + ellipse_scale, + graphviz_engine, + verbose_cov_errors, + ) + + # 3. Add initial traces to the figure (GT first, then initial dynamic traces) + # These define the plot state BEFORE the animation starts or when slider is at 0 + for trace in gt_traces: + fig.add_trace(trace) + for trace in initial_dynamic_traces: + fig.add_trace(trace) + + # 4. Generate frames for the animation (k=0 to num_steps) frames = [] - initial_dynamic_traces, initial_shapes, initial_image = [], [], None steps_iterable = range(num_steps + 1) try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") - except NameError: + except NameError: # Handle if tqdm is not available (e.g. standard Python script) pass + except Exception as e: # Catch other potential tqdm issues + print( + f"Note: Could not initialize tqdm progress bar ({e}). Continuing without it." + ) for k in steps_iterable: frame_data = next((item for item in history if item.step_index == k), None) if frame_data is None: - print(f"Warning: Missing data for step {k} in history.") + print(f"Warning: Missing data for step {k} in history. Skipping frame.") + # Optionally create an empty frame or reuse previous frame's data + # For simplicity, we skip here, which might cause jumps in animation continue - # Pass the engine choice to the content generator - frame_traces, frame_shapes, layout_image = generate_frame_content( + # Generate dynamic content for this specific frame + frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content( frame_data, X, L, @@ -427,31 +485,23 @@ def create_slam_animation( verbose_cov_errors, ) - if k == 0: - initial_dynamic_traces, initial_shapes, initial_image = ( - frame_traces, - frame_shapes, - layout_image, - ) - + # IMPORTANT: Combine static GT traces with the dynamic traces for THIS frame + # The 'data' in the frame replaces the figure's data for that step. frames.append( go.Frame( - data=frame_traces, + data=gt_traces + frame_dynamic_traces, # Include GT in every frame name=str(k), - layout=go.Layout( + layout=go.Layout( # Only update shapes and images in layout per frame shapes=frame_shapes, images=[layout_image] if layout_image else [] ), ) ) - # 3. Add initial dynamic traces (Unchanged) - for trace in initial_dynamic_traces: - fig.add_trace(trace) - - # 4. Assign frames (Unchanged) + # 5. Assign frames to the figure fig.update(frames=frames) - # 5. Configure layout (Unchanged) + # 6. Configure overall layout (sliders, buttons, axes, etc.) + # Pass the shapes and image corresponding to the INITIAL state (k=0) configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.") diff --git a/python/gtsam/examples/simulation.py b/python/gtsam/examples/simulation.py index a36433878..66beaa1ba 100644 --- a/python/gtsam/examples/simulation.py +++ b/python/gtsam/examples/simulation.py @@ -103,7 +103,7 @@ def generate_simulation_data( # Check sensor limits (range and Field of View - e.g. +/- 45 degrees) if ( true_range <= max_sensor_range - and abs(true_bearing.theta()) < np.pi / 4 + and abs(true_bearing.theta()) < np.pi / 2 ): # Sample noise noise_vec = measurement_noise_sampler.sample()