diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c13defbad..adde6d91d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -669,17 +669,17 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { }; #include +// This class is not available in python, just use a dictionary class FixedLagSmootherKeyTimestampMapValue { FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); }; +// This class is not available in python, just use a dictionary class FixedLagSmootherKeyTimestampMap { FixedLagSmootherKeyTimestampMap(); FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - // Note: no print function - // common STL methods size_t size() const; bool empty() const; @@ -739,6 +739,7 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { void print(string s = "IncrementalFixedLagSmoother:\n") const; + gtsam::Matrix marginalCovariance(size_t key) const; gtsam::ISAM2Params params() const; gtsam::NonlinearFactorGraph getFactors() const; diff --git a/gtsam/symbolic/symbolic.md b/gtsam/symbolic/symbolic.md index bfae60842..cc2f78d31 100644 --- a/gtsam/symbolic/symbolic.md +++ b/gtsam/symbolic/symbolic.md @@ -1,4 +1,4 @@ -# Symbolic Module +# Symbolic The `symbolic` module in GTSAM deals with the *structure* of factor graphs and Bayesian networks, independent of the specific numerical types of factors (like Gaussian or discrete). It allows for analyzing graph connectivity, determining optimal variable elimination orders, and understanding the sparsity structure of the resulting inference objects. diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 48a262f5e..fc3642728 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -2,15 +2,15 @@ "cells": [ { "cell_type": "markdown", - "id": "27cd96fc", + "id": "title-cell-fls", "metadata": {}, "source": [ - "# EKF-SLAM using Factor Graphs (SRIF-like)" + "# EKF-SLAM (using Incremental Fixed-Lag Smoother)" ] }, { "cell_type": "markdown", - "id": "copyright-cell-srif", + "id": "copyright-cell-fls", "metadata": { "tags": [ "remove-cell" @@ -27,7 +27,7 @@ }, { "cell_type": "markdown", - "id": "colab-button-cell-srif", + "id": "colab-button-cell-fls", "metadata": {}, "source": [ "\"Open" @@ -35,49 +35,19 @@ }, { "cell_type": "markdown", - "id": "intro-srif", + "id": "intro-fls-short", "metadata": {}, "source": [ - "This notebook demonstrates 2D SLAM using GTSAM's factor graph tools in an **iterative, filter-like manner**, similar in structure to a Square Root Information Filter (SRIF) or an EKF, but leveraging factor graph elimination instead of explicit matrix operations.\n", + "This notebook demonstrates 2D Simultaneous Localization and Mapping (SLAM) using an EKF, although it is implemented using GTSAM's `IncrementalFixedLagSmoother`, just using a lag of 1.\n", "\n", - "**Scenario:**\n", - "A robot moves in a circular path within a 10x10 meter area containing randomly placed landmarks. It receives noisy odometry and bearing-range measurements.\n", + "**Scenario:** A robot moves in a circular path, receiving noisy odometry and bearing-range measurements to landmarks.\n", "\n", - "**Factor Graph Filtering Approach:**\n", - "Instead of maintaining a large state vector and covariance matrix explicitly (like in classic EKF), we use GTSAM's `NonlinearFactorGraph` and `GaussianFactorGraph`:\n", - "\n", - "1. **Belief Representation:** The robot's belief (estimated state and uncertainty) after each step is implicitly represented by a factor graph (or the resulting Bayes Net after elimination).\n", - "2. **Prediction:**\n", - " * Start with the factors representing the belief from the *previous* step.\n", - " * Add a new `BetweenFactorPose2` representing the noisy odometry measurement, connecting the previous pose `X(k)` to the new pose `X(k+1)`.\n", - " * Linearize all relevant factors around the current best estimate (`Values`).\n", - " * Eliminate the resulting `GaussianFactorGraph` using an ordering that marginalizes out the previous pose `X(k)`, yielding a `GaussianBayesNet` representing the *predictive* distribution over landmarks and `X(k+1)`.\n", - "3. **Update:**\n", - " * Start with the factors representing the *predictive* belief.\n", - " * For each landmark measurement at the new pose `X(k+1)`:\n", - " * If the landmark `L(j)` is new, add it to the `Values` (initial estimate).\n", - " * Add a `BearingRangeFactor2D` connecting `X(k+1)` and `L(j)`.\n", - " * Linearize new/updated factors.\n", - " * Eliminate the `GaussianFactorGraph` to incorporate the measurement information, yielding a `GaussianBayesNet` representing the *posterior* belief.\n", - " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", - "4. **Iteration:** Repeat prediction and update for each time step.\n", - "\n", - "**Enhancements in this version:**\n", - "* Simulation code is moved to `simulation.py`.\n", - "* Plotting code is moved to `gtsam_plotly.py`.\n", - "* The evolving factor graph is displayed at each step using `graphviz`.\n", - "\n", - "**Advantages:**\n", - "* Leverages GTSAM's robust factor graph machinery.\n", - "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", - "* Avoids explicit management of large, dense covariance matrices.\n", - "\n", - "**Output:** The process will be visualized with the evolving factor graph, and finally using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + "**Approach:** We use a fixed-lag smoother which maintains and optimizes only a recent window of variables (defined by the `SMOOTHER_LAG`). Variables older than the lag are marginalized out, keeping the computational cost bounded, making it suitable for online applications. By default we set the lag to *1* here, which makes this an extended Kalman filter. But **feel free to change the lag and see the fixed-lag smoother results**." ] }, { "cell_type": "markdown", - "id": "setup-imports-srif", + "id": "setup-imports-fls", "metadata": {}, "source": [ "## 1. Setup and Imports" @@ -85,8 +55,8 @@ }, { "cell_type": "code", - "execution_count": null, - "id": "install-code-srif", + "execution_count": 1, + "id": "install-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -100,8 +70,8 @@ }, { "cell_type": "code", - "execution_count": null, - "id": "imports-code-srif", + "execution_count": 2, + "id": "imports-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -112,22 +82,28 @@ "import gtsam\n", "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", "\n", + "# Import the IncrementalFixedLagSmoother\n", + "from gtsam import IncrementalFixedLagSmoother\n", + "\n", + "# Helper modules\n", "import simulation\n", "from gtsam_plotly import SlamFrameData, create_slam_animation" ] }, { "cell_type": "markdown", - "id": "params-srif", + "id": "params-fls", "metadata": {}, "source": [ - "## 2. Simulation Parameters" + "## 2. Simulation and Smoother Parameters\n", + "\n", + "Define parameters for the simulation environment, robot motion, noise models, and the fixed-lag smoother." ] }, { "cell_type": "code", - "execution_count": null, - "id": "params-code-srif", + "execution_count": 9, + "id": "params-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -138,37 +114,51 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", - "DT = 1.0 # Time step duration (simplified)\n", + "NUM_STEPS = 50\n", + "DT = 1.0 # Time step duration\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", - "# Prior noise on the first pose (x, y, theta)\n", "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, np.deg2rad(1.0)]))\n", - "# Odometry noise (dx, dy, dtheta)\n", "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.05, np.deg2rad(2.0)]))\n", - "# Measurement noise (bearing, range)\n", "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0" + "MAX_SENSOR_RANGE = 5.0\n", + "\n", + "# --- Fixed-Lag Smoother Parameters ---\n", + "# Define the length of the smoothing window in seconds.\n", + "# A lag of 1*DT (e.g., 1.0 second) means the smoother maintains the state\n", + "# estimate for the current time step only, behaving like a filter.\n", + "# Larger lags incorporate more past information for smoothing.\n", + "SMOOTHER_LAG = 0.99 * DT\n" ] }, { "cell_type": "markdown", - "id": "ground-truth-srif", + "id": "ground-truth-fls", "metadata": {}, "source": [ "## 3. Generate Ground Truth Data\n", "\n", - "Use the `simulation` module to create true landmark positions, robot path, and simulate noisy measurements." + "Create the true environment, robot path, and simulate noisy sensor readings using the `simulation` module." ] }, { "cell_type": "code", - "execution_count": null, - "id": "ground-truth-call-srif", + "execution_count": 10, + "id": "ground-truth-call-fls", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Simulation Generated: 15 landmarks.\n", + "Simulation Generated: 51 ground truth poses and 50 odometry measurements.\n", + "Simulation Generated: 210 bearing-range measurements.\n" + ] + } + ], "source": [ "landmarks_gt_dict, poses_gt, odometry_measurements, measurements_sim, landmarks_gt_array = \\\n", " simulation.generate_simulation_data(\n", @@ -188,159 +178,16215 @@ }, { "cell_type": "markdown", - "id": "factor-graph-slam-impl", + "id": "smoother-impl", "metadata": {}, "source": [ - "## 4. Iterative Factor Graph SLAM Implementation\n", + "## 4. Fixed-Lag Smoother SLAM Implementation" + ] + }, + { + "cell_type": "markdown", + "id": "smoother-init", + "metadata": {}, + "source": [ + "### Initialize Smoother and Helper Functions\n", "\n", - "Here we perform the step-by-step prediction and update using factor graphs. The evolving graph will be displayed." + "We create the `IncrementalFixedLagSmoother` with the specified lag. We also initialize the first state (pose X(0) at time 0.0) and add it to the smoother using its `update` method. The `update` method requires factors, initial values (theta), and timestamps for the *new* variables being added." ] }, { "cell_type": "code", - "execution_count": null, - "id": "90bf9320", + "execution_count": 11, + "id": "smoother-init-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initializing IncrementalFixedLagSmoother with lag = 0.99 seconds...\n", + "Performing initial smoother update...\n", + "Initial update complete.\n" + ] + } + ], "source": [ + "# Helper for graphviz visualization\n", "WRITER = gtsam.GraphvizFormatting()\n", "WRITER.binaryEdges = True\n", "\n", - "def mage_dot(graph, estimate):\n", - " return graph.dot(estimate, writer=WRITER)\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "factor-graph-init-code", - "metadata": {}, - "outputs": [], - "source": [ - "# --- Initialization ---\n", - "current_estimate = gtsam.Values() # Stores current best estimates (linearization points)\n", - "current_graph = gtsam.NonlinearFactorGraph() # Stores factors added so far\n", - "current_pose_key = X(0)\n", + "def make_dot(graph, estimate):\n", + " # Visualize the factor graph currently managed by the smoother\n", + " WRITER.boxes = {key for key in estimate.keys() if gtsam.symbolChr(key) == ord('l')}\n", + " return graph.dot(estimate, writer=WRITER)\n", "\n", - "# Add prior on the initial pose X(0)\n", - "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", - "# Add a bit of noise to the initial guess if desired:\n", - "# initial_pose_noisy = initial_pose.compose(gtsam.Pose2(0.1, 0.1, 0.01))\n", - "# current_estimate.insert(current_pose_key, initial_pose_noisy)\n", - "current_estimate.insert(current_pose_key, initial_pose) \n", - "\n", - "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", + "# --- Smoother Initialization ---\n", + "print(f\"Initializing IncrementalFixedLagSmoother with lag = {SMOOTHER_LAG} seconds...\")\n", + "# Use ISAM2 parameters if specific settings are needed, otherwise defaults are used.\n", + "isam2_params = gtsam.ISAM2Params()\n", + "smoother = IncrementalFixedLagSmoother(SMOOTHER_LAG, isam2_params)\n", "\n", "# Variables to store results for animation\n", - "history = [] # Store SLAMFrameData objects for each step\n", - "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", + "history = []\n", "\n", - "# Initial marginals (only for X(0))\n", - "initial_gaussian_graph = current_graph.linearize(current_estimate)\n", - "initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", - "history.append(SlamFrameData(0, current_estimate, initial_marginals, mage_dot(current_graph, current_estimate)))" + "# --- Initial Step (k=0) ---\n", + "initial_pose_key = X(0)\n", + "initial_time = 0.0\n", + "initial_pose = poses_gt[0] # Start at ground truth (can add noise if desired)\n", + "\n", + "# Create containers for the first update\n", + "initial_factors = gtsam.NonlinearFactorGraph()\n", + "initial_values = gtsam.Values()\n", + "# The KeyTimestampMap maps variable keys (size_t) to their timestamps (double)\n", + "initial_timestamps = {}\n", + "\n", + "# Add prior factor for the first pose\n", + "initial_factors.add(gtsam.PriorFactorPose2(initial_pose_key, initial_pose, PRIOR_NOISE))\n", + "\n", + "# Add the initial pose estimate to Values\n", + "initial_values.insert(initial_pose_key, initial_pose)\n", + "\n", + "# Add the timestamp for the initial pose\n", + "initial_timestamps[initial_pose_key] = initial_time\n", + "\n", + "# Update the smoother with the initial state\n", + "print(\"Performing initial smoother update...\")\n", + "smoother.update(initial_factors, initial_values, initial_timestamps)\n", + "print(\"Initial update complete.\")\n", + "\n", + "# Store initial state for animation\n", + "current_estimate = smoother.calculateEstimate()\n", + "current_graph = smoother.getFactors() # Get factors currently managed by smoother\n", + "# ISAM can serve as marginals object:\n", + "current_marginals = smoother.getISAM2()\n", + "\n", + "history.append(SlamFrameData(0, current_estimate, current_marginals, make_dot(current_graph, current_estimate)))" ] }, { "cell_type": "markdown", - "id": "factor-graph-loop", + "id": "smoother-loop", "metadata": {}, "source": [ - "### Main Iterative Loop" + "### Main Iterative Loop\n", + "\n", + "At each step `k`, we process the odometry measurement from `X(k)` to `X(k+1)` and all landmark measurements taken *at* pose `X(k+1)`.\n", + "\n", + "1. **Prepare Data:** Collect new factors (`NonlinearFactorGraph`), initial estimates for *new* variables (`Values`), and timestamps for *new* variables (`KeyTimestampMap`) for the current step.\n", + "2. **Predict:** Calculate an initial estimate for the new pose `X(k+1)` based on the previous estimate `X(k)` (retrieved from the smoother) and the odometry measurement.\n", + "3. **Initialize Landmarks:** If a landmark is observed for the first time, calculate an initial estimate based on the predicted pose and the measurement, and add it to the `new_values` and `new_timestamps`.\n", + "4. **Update Smoother:** Call `smoother.update()` with the collected factors, values, and timestamps. This incorporates the new information, performs optimization (iSAM2), and marginalizes old variables.\n", + "5. **Store Results:** Retrieve the current estimate and factor graph from the smoother for visualization." ] }, { "cell_type": "code", - "execution_count": null, - "id": "factor-graph-loop-code", + "execution_count": 12, + "id": "smoother-loop-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Running Incremental Fixed-Lag Smoother SLAM loop (50 steps)...\n" + ] + }, + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "65c6048ecb324a96ac367007900bb015", + "version_major": 2, + "version_minor": 0 + }, + "text/plain": [ + " 0%| | 0/50 [00:00 str: - """Generates SVG path string for an ellipse directly from 2x2 covariance.""" - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite + """Generates SVG path string for an ellipse from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite 2x2 try: eigvals, eigvecs = np.linalg.eigh(cov) - # eigh sorts eigenvalues in ascending order eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues - minor_std, major_std = np.sqrt(eigvals) - v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] # Corresponding eigenvectors + minor_std, major_std = np.sqrt(eigvals) # eigh sorts ascending + v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] except np.linalg.LinAlgError: # Fallback to a small circle if decomposition fails radius = 0.1 * scale @@ -43,8 +42,7 @@ def create_ellipse_path_from_cov( x_p = cx + radius * np.cos(t) y_p = cy + radius * np.sin(t) else: - # Parametric equation: Center + scale * major_std * v_major * cos(t) + scale * minor_std * v_minor * sin(t) - # Note: We use scale*std which corresponds to half-axis lengths a,b used before + # Parametric equation using eigenvectors and eigenvalues t = np.linspace(0, 2 * np.pi, N) cos_t, sin_t = np.cos(t), np.sin(t) x_p = cx + scale * ( @@ -66,7 +64,9 @@ def create_ellipse_path_from_cov( # --- Plotly Element Generators --- -def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: +def create_gt_landmarks_trace( + landmarks_gt: Optional[np.ndarray], +) -> Optional[go.Scatter]: """Creates scatter trace for ground truth landmarks.""" if landmarks_gt is None or landmarks_gt.size == 0: return None @@ -79,7 +79,7 @@ def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: ) -def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: +def create_gt_path_trace(poses_gt: Optional[List[gtsam.Pose2]]) -> Optional[go.Scatter]: """Creates line trace for ground truth path.""" if not poses_gt: return None @@ -95,21 +95,36 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: def create_est_path_trace( est_path_x: List[float], est_path_y: List[float] ) -> go.Scatter: - """Creates trace for the estimated path.""" + """Creates trace for the estimated path (all poses up to current).""" return go.Scatter( x=est_path_x, y=est_path_y, mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), + line=dict(color="rgba(255, 0, 0, 0.3)", width=1), # Fainter line for history + marker=dict(size=4, color="red"), # Keep markers prominent name="Path Est", ) +def create_current_pose_trace( + current_pose: Optional[gtsam.Pose2], +) -> Optional[go.Scatter]: + """Creates a single marker trace for the current estimated pose.""" + if current_pose is None: + return None + return go.Scatter( + x=[current_pose.x()], + y=[current_pose.y()], + mode="markers", + marker=dict(color="magenta", size=10, symbol="cross"), + name="Current Pose", + ) + + def create_est_landmarks_trace( est_landmarks_x: List[float], est_landmarks_y: List[float] ) -> Optional[go.Scatter]: - """Creates trace for estimated landmarks.""" + """Creates trace for currently estimated landmarks.""" if not est_landmarks_x: return None return go.Scatter( @@ -122,7 +137,7 @@ def create_est_landmarks_trace( def _create_ellipse_shape_dict( - cx, cy, cov, scale, fillcolor, line_color + cx: float, cy: float, cov: np.ndarray, scale: float, fillcolor: str, line_color: str ) -> Dict[str, Any]: """Helper: Creates dict for a Plotly ellipse shape from covariance.""" path = create_ellipse_path_from_cov(cx, cy, cov, scale) @@ -133,6 +148,7 @@ def _create_ellipse_shape_dict( yref="y", fillcolor=fillcolor, line_color=line_color, + opacity=0.7, # Make ellipses slightly transparent ) @@ -145,8 +161,8 @@ def create_pose_ellipse_shape( cy=pose_mean_xy[1], cov=pose_cov, scale=scale, - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", + fillcolor="rgba(255,0,255,0.2)", # Magenta fill + line_color="rgba(255,0,255,0.5)", # Magenta line ) @@ -159,8 +175,8 @@ def create_landmark_ellipse_shape( cy=lm_mean_xy[1], cov=lm_cov, scale=scale, - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", + fillcolor="rgba(0,0,255,0.1)", # Blue fill + line_color="rgba(0,0,255,0.3)", # Blue line ) @@ -171,18 +187,12 @@ def dot_string_to_base64_svg( if not dot_string: return None 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" + source = graphviz.Source(dot_string, engine=engine) 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}") @@ -201,65 +211,77 @@ def generate_frame_content( ) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: """Generates dynamic traces, shapes, and layout image for a single frame.""" k = frame_data.step_index + # Use the results specific to this frame for current elements step_results = frame_data.results step_marginals = frame_data.marginals - frame_dynamic_traces: List[go.Scatter] = [] # Renamed for clarity + frame_dynamic_traces: List[go.Scatter] = [] frame_shapes: List[Dict[str, Any]] = [] layout_image: Optional[Dict[str, Any]] = None - # 1. Estimated Path - est_path_x = [ - step_results.atPose2(X(i)).x() - for i in range(k + 1) - if step_results.exists(X(i)) - ] - est_path_y = [ - step_results.atPose2(X(i)).y() - for i in range(k + 1) - if step_results.exists(X(i)) - ] - frame_dynamic_traces.append(create_est_path_trace(est_path_x, est_path_y)) + # 1. Estimated Path (Full History or Partial) + est_path_x = [] + est_path_y = [] + current_pose_est = None - # 2. Estimated Landmarks + # Plot poses currently existing in the step_results (e.g., within lag) + for i in range(k + 1): # Check poses up to current step index + pose_key = X(i) + if step_results.exists(pose_key): + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + if i == k: + current_pose_est = pose + + path_trace = create_est_path_trace(est_path_x, est_path_y) + if path_trace: + frame_dynamic_traces.append(path_trace) + + # Add a distinct marker for the current pose estimate + current_pose_trace = create_current_pose_trace(current_pose_est) + if current_pose_trace: + frame_dynamic_traces.append(current_pose_trace) + + # 2. Estimated Landmarks (Only those present in step_results) est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) + # Check existence in the results for the *current frame* if step_results.exists(lm_key): lm_point = step_results.atPoint2(lm_key) est_landmarks_x.append(lm_point[0]) est_landmarks_y.append(lm_point[1]) - landmark_keys.append(lm_key) + landmark_keys.append(lm_key) # Store keys for covariance lookup + lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: frame_dynamic_traces.append(lm_trace) - # 3. Covariance Ellipses + # 3. Covariance Ellipses (Only for items in step_results AND step_marginals) if step_marginals: + # Current Pose Ellipse pose_key = X(k) - if step_results.exists(pose_key): - try: - 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 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}") + # Retrieve cov from marginals specific to this frame + cov = step_marginals.marginalCovariance(pose_key) + # Ensure mean comes from the pose in current results + mean = step_results.atPose2(pose_key).translation() + frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) + # Landmark Ellipses (Iterate over keys found in step_results) for lm_key in landmark_keys: try: + # Retrieve cov from marginals specific to this frame cov = step_marginals.marginalCovariance(lm_key) + # Ensure mean comes from the landmark in current results mean = step_results.atPoint2(lm_key) frame_shapes.append( create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) - except RuntimeError as e: # More specific exception type + except RuntimeError: # Covariance might not be available if verbose: print( - f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov not in marginals @ step {k}" ) except Exception as e: if verbose: @@ -267,9 +289,8 @@ def generate_frame_content( f"Warn: LM {gtsam.Symbol(lm_key).index()} cov OTHER err @ step {k}: {e}" ) - # 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 + # 4. Graph Image for Layout + img_src = dot_string_to_base64_svg( frame_data.graph_dot_string, engine=graphviz_engine ) if img_src: @@ -284,10 +305,10 @@ def generate_frame_content( xanchor="left", yanchor="top", layer="below", - sizing="contain", # Contain should work well with SVG + sizing="contain", ) - # Return only the things that CHANGE frame-to-frame + # Return dynamic elements for this frame return frame_dynamic_traces, frame_shapes, layout_image @@ -336,7 +357,7 @@ def configure_figure_layout( x=plot_domain[0], xanchor="left", y=0, - yanchor="top", # Position relative to plot area + yanchor="top", buttons=[ dict( label="Play", @@ -371,35 +392,35 @@ def configure_figure_layout( title="Factor Graph SLAM Animation (Graph Left, Results Right)", xaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - domain=plot_domain, # Confine axis to right pane + domain=plot_domain, constrain="domain", ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], scaleanchor="x", scaleratio=1, - domain=[0, 1], # Full height within its domain + domain=[0, 1], ), width=1000, - height=600, # Adjust size for side-by-side + height=600, hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Initial shapes for the SLAM plot - images=( - [initial_image] if initial_image else [] - ), # Initial image for the left pane - showlegend=False, # Forego legend for space - # Autosize=True might help responsiveness but can interfere with domain + shapes=initial_shapes, # Initial shapes (frame 0) + images=([initial_image] if initial_image else []), # Initial image (frame 0) + showlegend=True, # Keep legend for clarity + legend=dict( + x=plot_domain[0], + y=1, + traceorder="normal", # Position legend + bgcolor="rgba(255,255,255,0.5)", + ), ) # --- Main Animation Orchestrator --- -# --- Main Animation Orchestrator (MODIFIED TO FIX GT DISPLAY) --- - - def create_slam_animation( history: List[SlamFrameData], X: Callable[[int], int], @@ -447,8 +468,7 @@ def create_slam_animation( 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 + # 3. Add initial traces (GT + dynamic frame 0) for trace in gt_traces: fig.add_trace(trace) for trace in initial_dynamic_traces: @@ -457,24 +477,12 @@ def create_slam_animation( # 4. Generate frames for the animation (k=0 to num_steps) frames = [] steps_iterable = range(num_steps + 1) - try: - steps_iterable = tqdm(steps_iterable, desc="Creating Frames") - 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." - ) + steps_iterable = tqdm(steps_iterable, desc="Creating Frames") 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. 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 - # Generate dynamic content for this specific frame + # Generate dynamic content specific to this frame frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content( frame_data, X, @@ -485,14 +493,18 @@ def create_slam_animation( verbose_cov_errors, ) - # 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. + # Frame definition: includes static GT + dynamic traces for this step + # Layout updates only include shapes and images for this step frames.append( go.Frame( - data=gt_traces + frame_dynamic_traces, # Include GT in every frame + data=gt_traces + + frame_dynamic_traces, # GT must be in each frame's data name=str(k), - layout=go.Layout( # Only update shapes and images in layout per frame - shapes=frame_shapes, images=[layout_image] if layout_image else [] + layout=go.Layout( + shapes=frame_shapes, # Replaces shapes list for this frame + images=( + [layout_image] if layout_image else [] + ), # Replaces image list ), ) ) @@ -501,7 +513,6 @@ def create_slam_animation( fig.update(frames=frames) # 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.")