Moved BatchFixedLagSmoother in right spot
Frank Dellaert
parent
b873550f77
commit
0078774266
|
|
@ -1,54 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "283174f8",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# BatchFixedLagSmoother Class Documentation\n",
|
||||
"\n",
|
||||
"*Disclaimer: This documentation was generated by AI and requires human revision for accuracy and completeness.*\n",
|
||||
"\n",
|
||||
"## Overview\n",
|
||||
"\n",
|
||||
"The `BatchFixedLagSmoother` class in GTSAM is a specialized smoother designed for fixed-lag smoothing in nonlinear factor graphs. It extends the capabilities of fixed-lag smoothing by maintaining a sliding window of the most recent variables and marginalizing out older variables. This is particularly useful in real-time applications where memory and computational efficiency are critical.\n",
|
||||
"\n",
|
||||
"## Key Functionalities\n",
|
||||
"\n",
|
||||
"### Smoothing and Optimization\n",
|
||||
"\n",
|
||||
"- **update**: This method is the core of the `BatchFixedLagSmoother`. It processes new factors and variables, updating the current estimate of the state. The update method also manages the marginalization of variables that fall outside the fixed lag window.\n",
|
||||
"\n",
|
||||
"### Factor Graph Management\n",
|
||||
"\n",
|
||||
"- **marginalize**: This function handles the marginalization of variables that are no longer within the fixed lag window. Marginalization is a crucial step in maintaining the size of the factor graph, ensuring that only relevant variables are kept for optimization.\n",
|
||||
"\n",
|
||||
"### Parameter Management\n",
|
||||
"\n",
|
||||
"- **Params**: The `Params` structure within the class allows users to configure various settings for the smoother, such as the lag duration and optimization parameters. This provides flexibility in tuning the smoother for specific applications.\n",
|
||||
"\n",
|
||||
"## Mathematical Formulation\n",
|
||||
"\n",
|
||||
"The `BatchFixedLagSmoother` operates on the principle of fixed-lag smoothing, where the objective is to estimate the state $\\mathbf{x}_t$ given all measurements up to time $t$, but only retaining a fixed window of recent states. The optimization problem can be expressed as:\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\min_{\\mathbf{x}_{t-L:t}} \\sum_{i=1}^{N} \\| \\mathbf{h}_i(\\mathbf{x}_{t-L:t}) - \\mathbf{z}_i \\|^2\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"where $L$ is the fixed lag, $\\mathbf{h}_i$ are the measurement functions, and $\\mathbf{z}_i$ are the measurements.\n",
|
||||
"\n",
|
||||
"## Usage Considerations\n",
|
||||
"\n",
|
||||
"- **Real-time Applications**: The `BatchFixedLagSmoother` is ideal for applications requiring real-time processing, such as robotics and autonomous vehicles, where the computational burden must be managed efficiently.\n",
|
||||
"- **Configuration**: Proper configuration of the lag duration and optimization parameters is essential for optimal performance. Users should experiment with different settings to achieve the desired balance between accuracy and computational load.\n",
|
||||
"\n",
|
||||
"## Conclusion\n",
|
||||
"\n",
|
||||
"The `BatchFixedLagSmoother` class provides a robust framework for fixed-lag smoothing in nonlinear systems. Its ability to efficiently manage the factor graph and perform real-time updates makes it a valuable tool in various applications requiring dynamic state estimation."
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 5
|
||||
}
|
||||
|
|
@ -33,9 +33,18 @@ public:
|
|||
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
|
||||
typedef std::shared_ptr<BatchFixedLagSmoother> shared_ptr;
|
||||
|
||||
/** default constructor */
|
||||
/**
|
||||
* Construct with parameters
|
||||
*
|
||||
* @param smootherLag The length of the smoother lag. Any variable older than this amount will be marginalized out.
|
||||
* @param parameters The L-M optimization parameters
|
||||
* @param enforceConsistency A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
|
||||
* linearization point of all variables involved in linearized/marginal factors at the edge of the
|
||||
* smoothing window.
|
||||
*/
|
||||
BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
|
||||
FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { }
|
||||
FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) {
|
||||
}
|
||||
|
||||
/** destructor */
|
||||
~BatchFixedLagSmoother() override {}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "283174f8",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# BatchFixedLagSmoother\n",
|
||||
"\n",
|
||||
"## Overview\n",
|
||||
"\n",
|
||||
"The `BatchFixedLagSmoother` class in GTSAM is designed for fixed-lag smoothing in nonlinear factor graphs. It maintains a sliding window of the most recent variables and marginalizes out older variables. This is particularly useful in real-time applications where memory and computational efficiency are critical.\n",
|
||||
"\n",
|
||||
"This fixed lag smoother will **batch-optimize** at every iteration, but warm-started from the last estimate."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "42c80522",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Mathematical Formulation\n",
|
||||
"\n",
|
||||
"The `BatchFixedLagSmoother` operates on the principle of fixed-lag smoothing, where the objective is to estimate the state $\\mathbf{x}_t$ given all measurements up to time $t$, but only retaining a fixed window of recent states. The optimization problem can be expressed as:\n",
|
||||
"$$\n",
|
||||
"\\min_{\\mathbf{x}_{t-L:t}} \\sum_{i=1}^{N} \\| \\mathbf{h}_i(\\mathbf{x}_{t-L:t}) - \\mathbf{z}_i \\|^2\n",
|
||||
"$$\n",
|
||||
"where $L$ is the fixed lag, $\\mathbf{h}_i$ are the measurement functions, and $\\mathbf{z}_i$ are the measurements.\n",
|
||||
"In practice, the functions $\\mathbf{h}_i$ depend only on a subset of the state variables $\\mathbf{X}_i$, and the optimization is performed over a set of $N$ *factors* $\\phi_i$ instead:\n",
|
||||
"$$\n",
|
||||
"\\min_{\\mathbf{x}_{t-L:t}} \\sum_{i=1}^{N} \\| \\phi_i(\\mathbf{X}_i; \\mathbf{z}_i) \\|^2\n",
|
||||
"$$\n",
|
||||
"The API below allows the user to add new factors at every iteration, which will be automatically pruned after they no longer depend on any variables in the lag."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "92b4f851",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## API\n",
|
||||
"\n",
|
||||
"### Constructor\n",
|
||||
"\n",
|
||||
"You construct a `BatchFixedLagSmoother` object with the following parameters:\n",
|
||||
"\n",
|
||||
"- **smootherLag**: The length of the smoother lag. Any variable older than this amount will be marginalized out. *(Default: 0.0)*\n",
|
||||
"- **parameters**: The Levenberg-Marquardt optimization parameters. *(Default: `LevenbergMarquardtParams()`)* \n",
|
||||
"- **enforceConsistency**: A flag indicating whether the optimizer should enforce probabilistic consistency by maintaining the linearization point of all variables involved in linearized/marginal factors at the edge of the smoothing window. *(Default: `true`)*\n",
|
||||
"\n",
|
||||
"### Smoothing and Optimization\n",
|
||||
"\n",
|
||||
"- **update**: This method is the core of the `BatchFixedLagSmoother`. It processes new factors and variables, updating the current estimate of the state. The update method also manages the marginalization of variables that fall outside the fixed lag window.\n",
|
||||
"\n",
|
||||
"### Computational Considerations\n",
|
||||
"\n",
|
||||
"Every call to `update` triggers a batch LM optimization: use the parameters to control the convergence thresholds to bound computation to fit within your application."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "4a2bdd3e",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Internals\n",
|
||||
"\n",
|
||||
"- **marginalize**: This function handles the marginalization of variables that are no longer within the fixed lag window. Marginalization is a crucial step in maintaining the size of the factor graph, ensuring that only relevant variables are kept for optimization.\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"language_info": {
|
||||
"name": "python"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 5
|
||||
}
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
# Nonlinear
|
||||
|
||||
The `nonlinear` module in GTSAM focuses on solving nonlinear optimization problems using factor graphs and incremental solvers.
|
||||
|
||||
Key Concepts:
|
||||
- **Nonlinear Factors**: Represent constraints or measurements in a nonlinear optimization problem.
|
||||
- **Factor Graphs**: A graphical representation of the optimization problem.
|
||||
- **Nonlinear Solvers**: Various optimization methods, typically calling linear solves in `linear`.
|
||||
|
||||
## Basics
|
||||
- **`NonlinearFactor.h`**: Defines the base classes for nonlinear factors, `NonlinearFactor` and `NoiseModelFactor`.
|
||||
- **`NonlinearFactorGraph.h`**: Implements a factor graph consisting of nonlinear factors.
|
||||
- **`Values.h`**: Stores variable assignments for optimization.
|
||||
|
||||
## Optimizers:
|
||||
- **`GaussNewtonOptimizer.h`**: Implements the Gauss-Newton optimization algorithm.
|
||||
- **`LevenbergMarquardtOptimizer.h`**: Provides the Levenberg-Marquardt optimization algorithm.
|
||||
- **`DoglegOptimizer.h`**: Implements the Dogleg optimization algorithm.
|
||||
|
||||
## Incremental Optimizers:
|
||||
- **`ISAM2.h`**: Implements the iSAM2 incremental solver.
|
||||
Loading…
Reference in New Issue