EKF-SLAM proto
Frank Dellaert
parent
e52685d441
commit
104bba2422
|
|
@ -0,0 +1,619 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "27cd96fc",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# EKF-SLAM using Factor Graphs (SRIF-like)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "copyright-cell-srif",
|
||||
"metadata": {
|
||||
"tags": [
|
||||
"remove-cell"
|
||||
]
|
||||
},
|
||||
"source": [
|
||||
"GTSAM Copyright 2010-2023, Georgia Tech Research Corporation,\n",
|
||||
"Atlanta, Georgia 30332-0415\n",
|
||||
"All Rights Reserved\n",
|
||||
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
|
||||
"\n",
|
||||
"See LICENSE for the license information"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "colab-button-cell-srif",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/examples/EKF_SLAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "intro-srif",
|
||||
"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",
|
||||
"\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",
|
||||
"\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",
|
||||
"**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 using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "setup-imports-srif",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## 1. Setup and Imports"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "install-code-srif",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# Install GTSAM and Plotly from pip if running in Google Colab\n",
|
||||
"try:\n",
|
||||
" import google.colab\n",
|
||||
" %pip install --quiet gtsam-develop plotly\n",
|
||||
"except ImportError:\n",
|
||||
" pass # Not in Colab"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "imports-code-srif",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import numpy as np\n",
|
||||
"import matplotlib.pyplot as plt # For initial plot if desired\n",
|
||||
"import plotly.graph_objects as go\n",
|
||||
"from tqdm.notebook import tqdm # Progress bar\n",
|
||||
"import math\n",
|
||||
"\n",
|
||||
"import gtsam\n",
|
||||
"from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "params-srif",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## 2. Simulation Parameters"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "params-code-srif",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# World parameters\n",
|
||||
"NUM_LANDMARKS = 15\n",
|
||||
"WORLD_SIZE = 10.0 # Environment bounds [-WORLD_SIZE/2, WORLD_SIZE/2]\n",
|
||||
"\n",
|
||||
"# 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",
|
||||
"\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",
|
||||
"# Create samplers for noise models\n",
|
||||
"prior_noise_sampler = gtsam.Sampler(PRIOR_NOISE, 42)\n",
|
||||
"odometry_noise_sampler = gtsam.Sampler(ODOMETRY_NOISE, 42)\n",
|
||||
"measurement_noise_sampler = gtsam.Sampler(MEASUREMENT_NOISE, 42)\n",
|
||||
"\n",
|
||||
"# Sensor parameters\n",
|
||||
"MAX_SENSOR_RANGE = 5.0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "ground-truth-srif",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## 3. Generate Ground Truth Data\n",
|
||||
"\n",
|
||||
"Create true landmark positions, robot path, and simulate noisy measurements."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "ground-truth-code-srif",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# 3.1 Ground Truth Landmarks\n",
|
||||
"landmarks_gt = (np.random.rand(2, NUM_LANDMARKS) - 0.5) * WORLD_SIZE\n",
|
||||
"landmark_ids = list(range(NUM_LANDMARKS))\n",
|
||||
"landmarks_gt_dict = {L(i): gtsam.Point2(landmarks_gt[:, i]) for i in range(NUM_LANDMARKS)}\n",
|
||||
"\n",
|
||||
"# 3.2 Ground Truth Robot Path\n",
|
||||
"poses_gt = []\n",
|
||||
"current_pose_gt = gtsam.Pose2(ROBOT_RADIUS, 0, np.pi / 2) # Start on circle edge\n",
|
||||
"poses_gt.append(current_pose_gt)\n",
|
||||
"\n",
|
||||
"for k in range(NUM_STEPS):\n",
|
||||
" delta_theta = ROBOT_ANGULAR_VEL * DT\n",
|
||||
" arc_length = ROBOT_ANGULAR_VEL * ROBOT_RADIUS * DT\n",
|
||||
" motion_command = gtsam.Pose2(arc_length, 0, delta_theta)\n",
|
||||
" current_pose_gt = current_pose_gt.compose(motion_command)\n",
|
||||
" poses_gt.append(current_pose_gt)\n",
|
||||
"\n",
|
||||
"# 3.3 Simulate Noisy Odometry Measurements (as Pose2 objects)\n",
|
||||
"odometry_measurements = []\n",
|
||||
"for k in range(NUM_STEPS):\n",
|
||||
" pose_k = poses_gt[k]\n",
|
||||
" pose_k1 = poses_gt[k+1]\n",
|
||||
" true_odom = pose_k.between(pose_k1)\n",
|
||||
" \n",
|
||||
" # Sample noise directly for Pose2 composition (approximate)\n",
|
||||
" # A more rigorous approach samples from the tangent space\n",
|
||||
" odom_noise_vec = odometry_noise_sampler.sample()\n",
|
||||
" noisy_odom = true_odom.compose(gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]))\n",
|
||||
" odometry_measurements.append(noisy_odom)\n",
|
||||
"\n",
|
||||
"# 3.4 Simulate Noisy Bearing-Range Measurements\n",
|
||||
"# measurements_sim[k] = list of (L(lm_id), range, bearing) for measurements *at* pose k\n",
|
||||
"measurements_sim = [[] for _ in range(NUM_STEPS + 1)]\n",
|
||||
"for k in range(NUM_STEPS + 1):\n",
|
||||
" robot_pose = poses_gt[k]\n",
|
||||
" for lm_id in range(NUM_LANDMARKS):\n",
|
||||
" lm_gt_pt = landmarks_gt_dict[L(lm_id)]\n",
|
||||
" try:\n",
|
||||
" # Use BearingRangeFactor2D to simulate the measurement model\n",
|
||||
" measurement_factor = gtsam.BearingRangeFactor2D(X(k), L(lm_id), \n",
|
||||
" robot_pose.bearing(lm_gt_pt),\n",
|
||||
" robot_pose.range(lm_gt_pt), \n",
|
||||
" MEASUREMENT_NOISE)\n",
|
||||
" true_range = measurement_factor.measured().range()\n",
|
||||
" true_bearing = measurement_factor.measured().bearing()\n",
|
||||
"\n",
|
||||
" if true_range <= MAX_SENSOR_RANGE and abs(true_bearing.theta())<np.pi/4:\n",
|
||||
" # Sample noise\n",
|
||||
" noise_vec = measurement_noise_sampler.sample()\n",
|
||||
" noisy_bearing = true_bearing.retract(np.array([noise_vec[0]])) # Retract on SO(2)\n",
|
||||
" noisy_range = true_range + noise_vec[1]\n",
|
||||
" \n",
|
||||
" if noisy_range > 0: # Ensure range is positive\n",
|
||||
" measurements_sim[k].append((L(lm_id), noisy_bearing, noisy_range))\n",
|
||||
" except Exception as e:\n",
|
||||
" # Catch potential errors like point being too close to the pose\n",
|
||||
" print(f\"Sim Warning at step {k}, landmark {lm_id}: {e}\")\n",
|
||||
" pass\n",
|
||||
"\n",
|
||||
"print(f\"Generated {NUM_LANDMARKS} landmarks.\")\n",
|
||||
"print(f\"Generated {NUM_STEPS} ground truth poses and odometry measurements.\")\n",
|
||||
"num_meas_total = sum(len(m_list) for m_list in measurements_sim)\n",
|
||||
"print(f\"Generated {num_meas_total} bearing-range measurements across all steps.\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "factor-graph-slam-impl",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## 4. Iterative Factor Graph SLAM Implementation\n",
|
||||
"\n",
|
||||
"Here we perform the step-by-step prediction and update using factor graphs."
|
||||
]
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"\n",
|
||||
"# Add prior on the initial pose X(0)\n",
|
||||
"initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n",
|
||||
"current_estimate.insert(current_pose_key, initial_pose)\n",
|
||||
"current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n",
|
||||
"\n",
|
||||
"# Variables to store results for animation\n",
|
||||
"results_history = [current_estimate] # Store Values object at each step\n",
|
||||
"marginals_history = [] # Store Marginals object at each step\n",
|
||||
"known_landmarks = set() # Set of landmark keys L(j) currently in the state\n",
|
||||
"\n",
|
||||
"# Initial marginals (only for X(0))\n",
|
||||
"try:\n",
|
||||
" initial_gaussian_graph = current_graph.linearize(current_estimate)\n",
|
||||
" initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n",
|
||||
" marginals_history.append(initial_marginals)\n",
|
||||
"except Exception as e:\n",
|
||||
" print(f\"Error computing initial marginals: {e}\")\n",
|
||||
" marginals_history.append(None) # Append placeholder if fails\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "factor-graph-loop",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Main Iterative Loop"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "factor-graph-loop-code",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"print(\"Running Iterative Factor Graph SLAM...\")\n",
|
||||
"\n",
|
||||
"for k in tqdm(range(NUM_STEPS)):\n",
|
||||
" # === Prediction Step ===\n",
|
||||
" prev_pose_key = X(k)\n",
|
||||
" current_pose_key = X(k + 1)\n",
|
||||
" odom_k = odometry_measurements[k]\n",
|
||||
" \n",
|
||||
" # Predict next pose and add to Values (linearization point)\n",
|
||||
" predicted_pose = current_estimate.atPose2(prev_pose_key).compose(odom_k)\n",
|
||||
" current_estimate.insert(current_pose_key, predicted_pose)\n",
|
||||
" \n",
|
||||
" # Add odometry factor to the graph\n",
|
||||
" current_graph.add(gtsam.BetweenFactorPose2(prev_pose_key, current_pose_key, odom_k, ODOMETRY_NOISE))\n",
|
||||
"\n",
|
||||
" # --- At this point, the full graph contains the history --- \n",
|
||||
" # --- To mimic SRIF/EKF, we *could* eliminate prev_pose_key, but --- \n",
|
||||
" # --- it's simpler here to keep the full graph and optimize it --- \n",
|
||||
" # --- then extract the current belief for the next step's factors --- \n",
|
||||
" # --- Let's proceed with adding measurements first ----\n",
|
||||
"\n",
|
||||
" # === Update Step ===\n",
|
||||
" measurements_k1 = measurements_sim[k + 1] # Measurements taken AT pose k+1\n",
|
||||
" landmarks_observed_this_step = set()\n",
|
||||
"\n",
|
||||
" for lm_key, measured_bearing, measured_range in measurements_k1:\n",
|
||||
" landmarks_observed_this_step.add(lm_key)\n",
|
||||
" \n",
|
||||
" # If landmark is new, initialize its estimate\n",
|
||||
" if not current_estimate.exists(lm_key):\n",
|
||||
" # Initial guess based on current pose estimate and measurement\n",
|
||||
" current_pose_val = current_estimate.atPose2(current_pose_key)\n",
|
||||
" delta_x = measured_range * math.cos(measured_bearing.theta())\n",
|
||||
" delta_y = measured_range * math.sin(measured_bearing.theta())\n",
|
||||
" # Transform delta from robot frame to world frame\n",
|
||||
" lm_initial_guess = current_pose_val.transformFrom(gtsam.Point2(delta_x, delta_y))\n",
|
||||
" current_estimate.insert(lm_key, lm_initial_guess)\n",
|
||||
" known_landmarks.add(lm_key)\n",
|
||||
" # print(f\"Step {k+1}: Initialized Landmark {lm_key.index()}\")\n",
|
||||
"\n",
|
||||
" # Add measurement factor\n",
|
||||
" current_graph.add(gtsam.BearingRangeFactor2D(current_pose_key, lm_key, \n",
|
||||
" measured_bearing, measured_range,\n",
|
||||
" MEASUREMENT_NOISE))\n",
|
||||
" \n",
|
||||
" # --- Optimization (Incremental Smoothing) ---\n",
|
||||
" # In a true filter, we'd eliminate previous states.\n",
|
||||
" # Here, we re-optimize the growing graph. For efficiency in large problems, \n",
|
||||
" # iSAM2 would be used, but Levenberg-Marquardt on the growing graph \n",
|
||||
" # demonstrates the concept for this smaller example.\n",
|
||||
" \n",
|
||||
" optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n",
|
||||
" try:\n",
|
||||
" current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n",
|
||||
" except Exception as e:\n",
|
||||
" print(f\"Optimization failed at step {k+1}: {e}\")\n",
|
||||
" # Potentially revert estimate update or handle error\n",
|
||||
" # For simplicity, we continue with the potentially unoptimized estimate\n",
|
||||
" pass \n",
|
||||
"\n",
|
||||
" # Store results for animation\n",
|
||||
" results_history.append(gtsam.Values(current_estimate)) # Store a copy\n",
|
||||
" \n",
|
||||
" # Calculate marginals for visualization (can be slow for large graphs)\n",
|
||||
" try:\n",
|
||||
" current_gaussian_graph = current_graph.linearize(current_estimate)\n",
|
||||
" current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n",
|
||||
" marginals_history.append(current_marginals)\n",
|
||||
" except Exception as e:\n",
|
||||
" print(f\"Marginals calculation failed at step {k+1}: {e}\")\n",
|
||||
" marginals_history.append(None) # Append placeholder\n",
|
||||
"\n",
|
||||
"print(\"Iterative Factor Graph SLAM finished.\")\n",
|
||||
"print(f\"Final number of poses: {k+2}\")\n",
|
||||
"print(f\"Number of landmarks mapped: {len(known_landmarks)}\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "plotly-animation",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## 5. Create Plotly Animation"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "plotly-animation-code",
|
||||
"metadata": {
|
||||
"scrolled": false,
|
||||
"tags": [
|
||||
"remove-input"
|
||||
]
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"print(\"Generating Plotly animation...\")\n",
|
||||
"\n",
|
||||
"def ellipse_path(cx, cy, sizex, sizey, angle, N=60):\n",
|
||||
" \"\"\"SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.\"\"\"\n",
|
||||
" angle_rad = np.radians(angle)\n",
|
||||
" t = np.linspace(0, 2 * np.pi, N)\n",
|
||||
" x = (sizex / 2) * np.cos(t)\n",
|
||||
" y = (sizey / 2) * np.sin(t)\n",
|
||||
"\n",
|
||||
" x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad)\n",
|
||||
" y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad)\n",
|
||||
"\n",
|
||||
" path = f\"M {x_rot[0]},{y_rot[0]} \" + \" \".join(\n",
|
||||
" f\"L{x_},{y_}\" for x_, y_ in zip(x_rot[1:], y_rot[1:])\n",
|
||||
" ) + \" Z\"\n",
|
||||
" return path\n",
|
||||
"\n",
|
||||
"# Helper to convert GTSAM covariance to Plotly ellipse parameters\n",
|
||||
"def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0):\n",
|
||||
" \"\"\"Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.\"\"\"\n",
|
||||
" # Ensure positive definite - add small epsilon if needed\n",
|
||||
" cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 \n",
|
||||
" try:\n",
|
||||
" eigvals, eigvecs = np.linalg.eigh(cov)\n",
|
||||
" # Ensure eigenvalues are positive for sqrt\n",
|
||||
" eigvals = np.maximum(eigvals, 1e-9)\n",
|
||||
" except np.linalg.LinAlgError:\n",
|
||||
" print(\"Warning: Covariance matrix SVD failed, using default ellipse.\")\n",
|
||||
" return 0, 0.1*scale, 0.1*scale # Default small ellipse\n",
|
||||
" \n",
|
||||
" # Width/Height are 2*scale*sqrt(eigenvalue)\n",
|
||||
" width = 2 * scale * np.sqrt(eigvals[1])\n",
|
||||
" height = 2 * scale * np.sqrt(eigvals[0])\n",
|
||||
" \n",
|
||||
" # Angle of the major axis (corresponding to largest eigenvalue)\n",
|
||||
" angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1])\n",
|
||||
" angle_deg = np.degrees(angle_rad)\n",
|
||||
" \n",
|
||||
" return angle_deg, width, height\n",
|
||||
"\n",
|
||||
"# --- Create Plotly Figure --- \n",
|
||||
"fig = go.Figure()\n",
|
||||
"\n",
|
||||
"# Add Ground Truth Landmarks (static)\n",
|
||||
"fig.add_trace(go.Scatter(\n",
|
||||
" x=landmarks_gt[0, :], \n",
|
||||
" y=landmarks_gt[1, :], \n",
|
||||
" mode='markers', \n",
|
||||
" marker=dict(color='black', size=8, symbol='star'),\n",
|
||||
" name='Landmarks GT'\n",
|
||||
"))\n",
|
||||
"\n",
|
||||
"# Add Ground Truth Path (static)\n",
|
||||
"gt_path_x = [p.x() for p in poses_gt]\n",
|
||||
"gt_path_y = [p.y() for p in poses_gt]\n",
|
||||
"fig.add_trace(go.Scatter(\n",
|
||||
" x=gt_path_x,\n",
|
||||
" y=gt_path_y,\n",
|
||||
" mode='lines',\n",
|
||||
" line=dict(color='gray', width=1, dash='dash'),\n",
|
||||
" name='Path GT'\n",
|
||||
"))\n",
|
||||
"\n",
|
||||
"# --- Animation Frames --- \n",
|
||||
"frames = []\n",
|
||||
"steps = list(range(NUM_STEPS + 1))\n",
|
||||
"\n",
|
||||
"for k in tqdm(steps):\n",
|
||||
" frame_data = []\n",
|
||||
" step_results = results_history[k]\n",
|
||||
" step_marginals = marginals_history[k]\n",
|
||||
" \n",
|
||||
" # Estimated Path up to step k\n",
|
||||
" est_path_x = [results_history[i].atPose2(X(i)).x() for i in range(k + 1)]\n",
|
||||
" est_path_y = [results_history[i].atPose2(X(i)).y() for i in range(k + 1)]\n",
|
||||
" frame_data.append(go.Scatter(\n",
|
||||
" x=est_path_x,\n",
|
||||
" y=est_path_y,\n",
|
||||
" mode='lines+markers',\n",
|
||||
" line=dict(color='red', width=2),\n",
|
||||
" marker=dict(size=4, color='red'),\n",
|
||||
" name='Path Est'\n",
|
||||
" ))\n",
|
||||
" \n",
|
||||
" # Estimated Landmarks known at step k\n",
|
||||
" est_landmarks_x = []\n",
|
||||
" est_landmarks_y = []\n",
|
||||
" landmark_keys_in_frame = []\n",
|
||||
" for lm_key in step_results.keys():\n",
|
||||
" if gtsam.symbolChr(lm_key)==108: # Check if it's a landmark key\n",
|
||||
" lm_pose = step_results.atPoint2(lm_key)\n",
|
||||
" est_landmarks_x.append(lm_pose[0])\n",
|
||||
" est_landmarks_y.append(lm_pose[1])\n",
|
||||
" landmark_keys_in_frame.append(lm_key)\n",
|
||||
" \n",
|
||||
" if est_landmarks_x:\n",
|
||||
" frame_data.append(go.Scatter(\n",
|
||||
" x=est_landmarks_x, \n",
|
||||
" y=est_landmarks_y, \n",
|
||||
" mode='markers',\n",
|
||||
" marker=dict(color='blue', size=6, symbol='x'),\n",
|
||||
" name='Landmarks Est'\n",
|
||||
" ))\n",
|
||||
"\n",
|
||||
" # Current Pose Covariance Ellipse\n",
|
||||
" shapes = [] # List to hold ellipse shapes for this frame\n",
|
||||
" if step_marginals is not None:\n",
|
||||
" try:\n",
|
||||
" current_pose_key = X(k)\n",
|
||||
" pose_cov = step_marginals.marginalCovariance(current_pose_key)\n",
|
||||
" pose_mean = step_results.atPose2(current_pose_key).translation()\n",
|
||||
" angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov)\n",
|
||||
" cx, cy = pose_mean[0], pose_mean[1]\n",
|
||||
" shapes.append(dict(\n",
|
||||
" type=\"path\",\n",
|
||||
" path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n",
|
||||
" xref=\"x\", yref=\"y\",\n",
|
||||
" fillcolor=\"rgba(255,0,255,0.2)\",\n",
|
||||
" line_color=\"rgba(255,0,255,0.5)\",\n",
|
||||
" name=f'Pose {k} Cov'\n",
|
||||
" ))\n",
|
||||
" except Exception as e:\n",
|
||||
" print(f\"Warning: Failed getting pose cov ellipse at step {k}: {e}\")\n",
|
||||
"\n",
|
||||
" # Landmark Covariance Ellipses\n",
|
||||
" for lm_key in landmark_keys_in_frame:\n",
|
||||
" index = gtsam.symbolIndex(lm_key)\n",
|
||||
" try:\n",
|
||||
" lm_cov = step_marginals.marginalCovariance(lm_key)\n",
|
||||
" lm_mean = step_results.atPoint2(lm_key)\n",
|
||||
" angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov)\n",
|
||||
" cx, cy = lm_mean[0], lm_mean[1]\n",
|
||||
" shapes.append(dict(\n",
|
||||
" type=\"path\",\n",
|
||||
" path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n",
|
||||
" xref=\"x\", yref=\"y\",\n",
|
||||
" fillcolor=\"rgba(0,0,255,0.1)\",\n",
|
||||
" line_color=\"rgba(0,0,255,0.3)\",\n",
|
||||
" name=f'LM {index} Cov'\n",
|
||||
" ))\n",
|
||||
" except Exception as e:\n",
|
||||
" print(f\"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}\")\n",
|
||||
"\n",
|
||||
" frames.append(go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)))\n",
|
||||
"\n",
|
||||
"# --- Set Initial State and Layout --- \n",
|
||||
"fig.update(frames=frames)\n",
|
||||
"\n",
|
||||
"# Set initial data to the first frame's data\n",
|
||||
"if frames:\n",
|
||||
" fig.add_traces(frames[0].data)\n",
|
||||
" initial_shapes = frames[0].layout.shapes if frames[0].layout else []\n",
|
||||
"else:\n",
|
||||
" initial_shapes = []\n",
|
||||
"\n",
|
||||
"# Define slider\n",
|
||||
"sliders = [dict(\n",
|
||||
" active=0,\n",
|
||||
" currentvalue={\"prefix\": \"Step: \"},\n",
|
||||
" pad={\"t\": 50},\n",
|
||||
" steps=[dict(label=str(k), \n",
|
||||
" method='animate', \n",
|
||||
" args=[[str(k)], \n",
|
||||
" dict(mode='immediate', \n",
|
||||
" frame=dict(duration=100, redraw=True), \n",
|
||||
" transition=dict(duration=0))])\n",
|
||||
" for k in steps]\n",
|
||||
")]\n",
|
||||
"\n",
|
||||
"# Update layout\n",
|
||||
"fig.update_layout(\n",
|
||||
" title='Iterative Factor Graph SLAM Animation',\n",
|
||||
" xaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], constrain='domain'),\n",
|
||||
" yaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], scaleanchor='x', scaleratio=1),\n",
|
||||
" width=800, height=800,\n",
|
||||
" hovermode='closest',\n",
|
||||
" updatemenus=[dict(type='buttons', \n",
|
||||
" showactive=False,\n",
|
||||
" buttons=[dict(label='Play',\n",
|
||||
" method='animate', \n",
|
||||
" args=[None, \n",
|
||||
" dict(mode='immediate', \n",
|
||||
" frame=dict(duration=100, redraw=True), \n",
|
||||
" transition=dict(duration=0), \n",
|
||||
" fromcurrent=True)])])],\n",
|
||||
" sliders=sliders,\n",
|
||||
" shapes=initial_shapes # Set initial shapes\n",
|
||||
")\n",
|
||||
"\n",
|
||||
"print(\"Displaying animation...\")\n",
|
||||
"fig.show()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "discussion-srif",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## 6. Discussion\n",
|
||||
"\n",
|
||||
"* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n",
|
||||
"* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n",
|
||||
"* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n",
|
||||
"* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)."
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.10.12"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 5
|
||||
}
|
||||
Loading…
Reference in New Issue