ConstantVelocityFactor + wrapping
Frank Dellaert
parent
8757090b7a
commit
12104b779c
|
|
@ -41,7 +41,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
|
|||
~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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,176 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# ConstantVelocityFactor\n",
|
||||
"\n",
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ConstantVelocityFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\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
|
||||
}
|
||||
|
|
@ -426,6 +426,12 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/ConstantVelocityFactor.h>
|
||||
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 <gtsam/navigation/Scenario.h>
|
||||
virtual class Scenario {
|
||||
gtsam::Pose3 pose(double t) const;
|
||||
|
|
|
|||
Loading…
Reference in New Issue