diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 9fe5bef85..49d97033e 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -41,7 +41,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN { ~ConstantVelocityFactor() override {} /** - * @brief Caclulate error: (x2 - x1.update(dt))) + * @brief Calculate error: (x2 - x1.update(dt))) * where X1 and X1 are NavStates and dt is * the time difference in seconds between the states. * @param x1 NavState for key a diff --git a/gtsam/navigation/doc/ConstantVelocityFactor.ipynb b/gtsam/navigation/doc/ConstantVelocityFactor.ipynb new file mode 100644 index 000000000..561357221 --- /dev/null +++ b/gtsam/navigation/doc/ConstantVelocityFactor.ipynb @@ -0,0 +1,176 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# ConstantVelocityFactor\n", + "\n", + "\"Open\n", + "\n", + "## Overview\n", + "\n", + "The `ConstantVelocityFactor` (contributed by [Asa Hammond](https://www.linkedin.com/in/asahammond/) in 2021) is a simple motion model factor that connects two `NavState` variables at different times, $t_i$ and $t_j$. It enforces the assumption that the velocity remained constant between the two time steps.\n", + "\n", + "Given `NavState` $X_i$ (containing pose $T_i$ and velocity $v_i$) and `NavState` $X_j$ (containing pose $T_j$ and velocity $v_j$), and the time difference $\\Delta t = t_j - t_i$, this factor penalizes deviations from the constant velocity prediction." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Mathematical Formulation\n", + "\n", + "The factor uses the `NavState::update` method (or equivalent logic internally) to predict the state at time $t_j$ based on the state at $t_i$ and the assumption of constant velocity (and zero acceleration/angular velocity). Let this prediction be $X_{j, pred}$:\n", + "$$ X_{j, pred} = X_i . \\text{update}(\\text{accel}=0, \\text{omega}=0, \\Delta t) $$ \n", + "Essentially, this integrates the velocity $v_i$ over $\\Delta t$ to predict the change in position, while keeping orientation and velocity constant:\n", + "$$ R_{j, pred} = R_i $$ \n", + "$$ v_{j, pred} = v_i $$ \n", + "$$ p_{j, pred} = p_i + R_i (v_i^{body}) \\Delta t \\quad \\text{(using body velocity update)} $$ \n", + "\n", + "The factor's 9-dimensional error $e$ is the difference between the predicted state $X_{j, pred}$ and the actual state $X_j$, expressed in the tangent space at $X_{j,pred}$:\n", + "$$ e = \\text{localCoordinates}_{X_{j, pred}}(X_j) $$ \n", + "The noise model associated with the factor determines how strongly deviations from this constant velocity prediction are penalized." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Key Functionality / API\n", + "\n", + "- **Constructor**: `ConstantVelocityFactor(key1, key2, dt, model)`: Creates the factor connecting the `NavState` at `key1` (time $i$) and `key2` (time $j$), given the time difference `dt` and a 9D noise model.\n", + "- **`evaluateError(state1, state2)`**: Calculates the 9D error vector based on the constant velocity prediction from `state1` to `state2` over the stored `dt`." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Usage Example\n", + "\n", + "This factor is often used as a simple process model between consecutive states when higher-fidelity IMU integration is not available or needed." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Created ConstantVelocityFactor:\n", + " keys = { x0 x1 }\n", + " noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.1; 0.1; 0.1; 0.05; 0.05; 0.05];\n", + "\n", + "Error for perfect prediction (should be zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n", + "Error for velocity change: [0. 0. 0. 0. 0. 0. 0. 0.1 0. ]\n", + "Error for position change: [0. 0. 0. 0. 0.05 0. 0. 0. 0. ]\n" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam.symbol_shorthand import X # Using X for NavState keys here\n", + "\n", + "# Define keys for two NavStates\n", + "key1 = X(0)\n", + "key2 = X(1)\n", + "\n", + "# Time difference between states\n", + "dt = 0.1 # seconds\n", + "\n", + "# Define a noise model - how much deviation from constant velocity is allowed?\n", + "# Example: Tighter on rotation/velocity, looser on position change due to velocity\n", + "rot_sigma = 0.01 # rad\n", + "pos_sigma = 0.1 # m \n", + "vel_sigma = 0.05 # m/s\n", + "sigmas = np.concatenate([\n", + " np.full(3, rot_sigma), \n", + " np.full(3, pos_sigma),\n", + " np.full(3, vel_sigma)\n", + "])\n", + "noise_model = gtsam.noiseModel.Diagonal.Sigmas(sigmas)\n", + "\n", + "# Create the factor\n", + "cv_factor = gtsam.ConstantVelocityFactor(key1, key2, dt, noise_model)\n", + "\n", + "print(\"Created ConstantVelocityFactor:\")\n", + "cv_factor.print()\n", + "\n", + "# --- Example Evaluation ---\n", + "# State 1: At origin, moving along +X at 1 m/s\n", + "pose1 = gtsam.Pose3()\n", + "vel1 = np.array([1.0, 0.0, 0.0])\n", + "state1 = gtsam.NavState(pose1, vel1)\n", + "\n", + "# State 2: Exactly matches constant velocity prediction\n", + "pose2 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.0, 0.0]))\n", + "vel2 = vel1 # Constant velocity\n", + "state2_perfect = gtsam.NavState(pose2, vel2)\n", + "\n", + "error_perfect = cv_factor.evaluateError(state1, state2_perfect)\n", + "print(\"\\nError for perfect prediction (should be zero):\", error_perfect)\n", + "\n", + "# State 3: Velocity changed slightly\n", + "vel3 = np.array([1.0, 0.1, 0.0]) # Added Y velocity\n", + "state3_vel_change = gtsam.NavState(pose2, vel3) # Keep predicted pose\n", + "\n", + "error_vel_change = cv_factor.evaluateError(state1, state3_vel_change)\n", + "print(\"Error for velocity change:\", error_vel_change)\n", + "\n", + "# State 4: Position is slightly off prediction\n", + "pose4 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.05, 0.0])) # Y pos is off\n", + "state4_pos_change = gtsam.NavState(pose4, vel2) # Keep velocity\n", + "\n", + "error_pos_change = cv_factor.evaluateError(state1, state4_pos_change)\n", + "print(\"Error for position change:\", error_pos_change)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Source\n", + "- [ConstantVelocityFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ConstantVelocityFactor.h)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "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.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 30bec16b9..f3d32251f 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -426,6 +426,12 @@ virtual class BarometricFactor : gtsam::NonlinearFactor { void serialize() const; }; +#include +class ConstantVelocityFactor : gtsam::NonlinearFactor { + ConstantVelocityFactor(size_t i, size_t j, double dt, const gtsam::noiseModel::Base* model); + gtsam::Vector evaluateError(const gtsam::NavState &x1, const gtsam::NavState &x2) const; +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const;