Reviewed 3 more
Frank Dellaert
parent
d31ab0f8f6
commit
0e68480e32
|
|
@ -57,7 +57,7 @@
|
||||||
"\n",
|
"\n",
|
||||||
"The `CustomFactor` class allows users to define a custom error function. In C++ it is defined as below:\n",
|
"The `CustomFactor` class allows users to define a custom error function. In C++ it is defined as below:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"```c++\n",
|
"```cpp\n",
|
||||||
"using JacobianVector = std::vector<Matrix>;\n",
|
"using JacobianVector = std::vector<Matrix>;\n",
|
||||||
"using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;\n",
|
"using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;\n",
|
||||||
"```\n",
|
"```\n",
|
||||||
|
|
@ -259,7 +259,7 @@
|
||||||
"This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.\n",
|
"This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The constructor of `CustomFactor` is\n",
|
"The constructor of `CustomFactor` is\n",
|
||||||
"```c++\n",
|
"```cpp\n",
|
||||||
"/**\n",
|
"/**\n",
|
||||||
"* Constructor\n",
|
"* Constructor\n",
|
||||||
"* @param noiseModel shared pointer to noise model\n",
|
"* @param noiseModel shared pointer to noise model\n",
|
||||||
|
|
@ -275,7 +275,7 @@
|
||||||
"At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.\n",
|
"At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Something that deserves a special mention is this:\n",
|
"Something that deserves a special mention is this:\n",
|
||||||
"```c++\n",
|
"```cpp\n",
|
||||||
"/*\n",
|
"/*\n",
|
||||||
" * NOTE\n",
|
" * NOTE\n",
|
||||||
" * ==========\n",
|
" * ==========\n",
|
||||||
|
|
|
||||||
|
|
@ -5,63 +5,42 @@
|
||||||
"id": "59407eaf",
|
"id": "59407eaf",
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"source": [
|
"source": [
|
||||||
"# ExpressionFactor Class Documentation\n",
|
"# ExpressionFactor\n",
|
||||||
"\n",
|
|
||||||
"*Disclaimer: This documentation was generated by AI and may require human revision for accuracy and completeness.*\n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"## Overview\n",
|
"## Overview\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The `ExpressionFactor` class in GTSAM is a template class designed to work with factor graphs in the context of nonlinear optimization. It represents a factor that can be constructed from an expression, allowing for flexible and efficient computation of error terms in optimization problems.\n",
|
"The `ExpressionFactor` class in GTSAM is a template class designed to work with factor graphs in the context of nonlinear optimization. It represents a factor that can be constructed from a [GTSAM expression](../../../doc/expressions.md), allowing for flexible and efficient computation of error terms in optimization problems.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"## Key Features\n",
|
"The `ExpressionFactor` class allows users to define factors based on expressions in C++, that use (reverse) automatic differentiation to compute their Jacobians.\n",
|
||||||
"\n",
|
|
||||||
"- **Expression-Based Factor**: The `ExpressionFactor` class allows users to define factors based on expressions, which can represent complex mathematical relationships between variables.\n",
|
|
||||||
"- **Error Calculation**: It computes the error based on the difference between the predicted and observed values, typically used in least-squares optimization.\n",
|
|
||||||
"- **Jacobian Computation**: The class can compute the Jacobian matrix, which is essential for gradient-based optimization methods.\n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"## Main Methods\n",
|
"## Main Methods\n",
|
||||||
"\n",
|
"\n",
|
||||||
"### Constructor\n",
|
"### Constructor\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The `ExpressionFactor` class provides constructors that allow for the initialization of the factor with a specific expression and measurement. The constructors are designed to handle various types of expressions and measurements, making the class versatile for different applications.\n",
|
"The `ExpressionFactor` class provides a constructor that allows for the initialization of the factor with a specific expression and measurement:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"### `evaluateError`\n",
|
"```cpp\n",
|
||||||
"\n",
|
" /**\n",
|
||||||
"This method calculates the error vector for the factor. The error is typically defined as the difference between the predicted value from the expression and the actual measurement. Mathematically, this can be represented as:\n",
|
" * Constructor: creates a factor from a measurement and measurement function\n",
|
||||||
"\n",
|
" * @param noiseModel the noise model associated with a measurement\n",
|
||||||
"$$\n",
|
" * @param measurement actual value of the measurement, of type T\n",
|
||||||
"\\text{error} = \\text{measurement} - \\text{expression}\n",
|
" * @param expression predicts the measurement from Values\n",
|
||||||
"$$\n",
|
" * The keys associated with the factor, returned by keys(), are sorted.\n",
|
||||||
"\n",
|
" */\n",
|
||||||
"where `measurement` is the observed value, and `expression` is the predicted value based on the current estimate of the variables.\n",
|
" ExpressionFactor(const SharedNoiseModel& noiseModel, //\n",
|
||||||
"\n",
|
" const T& measurement, const Expression<T>& expression)\n",
|
||||||
"### `linearize`\n",
|
" : NoiseModelFactor(noiseModel), measured_(measurement) {\n",
|
||||||
"\n",
|
" initialize(expression);\n",
|
||||||
"The `linearize` method is used to linearize the factor around a given linearization point. This involves computing the Jacobian matrix, which represents the partial derivatives of the error with respect to the variables. The Jacobian is crucial for iterative optimization algorithms such as Gauss-Newton or Levenberg-Marquardt.\n",
|
" }\n",
|
||||||
"\n",
|
"```"
|
||||||
"### `clone`\n",
|
|
||||||
"\n",
|
|
||||||
"The `clone` method creates a deep copy of the factor. This is useful when factors need to be duplicated, ensuring that changes to one copy do not affect the other.\n",
|
|
||||||
"\n",
|
|
||||||
"## Mathematical Background\n",
|
|
||||||
"\n",
|
|
||||||
"The `ExpressionFactor` class is grounded in the principles of nonlinear optimization, particularly in the context of factor graphs. Factor graphs are bipartite graphs used to represent the factorization of a function, often used in probabilistic graphical models and optimization problems.\n",
|
|
||||||
"\n",
|
|
||||||
"In the context of GTSAM, factors represent constraints or relationships between variables. The `ExpressionFactor` allows these relationships to be defined using mathematical expressions, providing a flexible and powerful tool for modeling complex systems.\n",
|
|
||||||
"\n",
|
|
||||||
"## Usage\n",
|
|
||||||
"\n",
|
|
||||||
"The `ExpressionFactor` class is typically used in scenarios where the relationships between variables can be naturally expressed as mathematical expressions. This includes applications in robotics, computer vision, and other fields where optimization problems are prevalent.\n",
|
|
||||||
"\n",
|
|
||||||
"By leveraging the power of expressions, users can define custom factors that capture the nuances of their specific problem, leading to more accurate and efficient optimization solutions.\n",
|
|
||||||
"\n",
|
|
||||||
"---\n",
|
|
||||||
"\n",
|
|
||||||
"This documentation provides a high-level overview of the `ExpressionFactor` class, highlighting its main features and methods. For detailed usage and examples, users should refer to the GTSAM library documentation and source code."
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"metadata": {},
|
"metadata": {
|
||||||
|
"language_info": {
|
||||||
|
"name": "python"
|
||||||
|
}
|
||||||
|
},
|
||||||
"nbformat": 4,
|
"nbformat": 4,
|
||||||
"nbformat_minor": 5
|
"nbformat_minor": 5
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -5,55 +5,31 @@
|
||||||
"id": "a1c00a8c",
|
"id": "a1c00a8c",
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"source": [
|
"source": [
|
||||||
"# ExpressionFactorGraph Class Documentation\n",
|
"# ExpressionFactorGraph \n",
|
||||||
"\n",
|
|
||||||
"*Disclaimer: This documentation was generated by AI and may require human revision for accuracy and completeness.*\n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"## Overview\n",
|
"## Overview\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The `ExpressionFactorGraph` class in GTSAM is a specialized factor graph designed to work with expressions. It extends the capabilities of a standard factor graph by allowing the incorporation of symbolic expressions, which can be particularly useful in applications requiring symbolic computation and automatic differentiation.\n",
|
"The `ExpressionFactorGraph` class in GTSAM is a specialized factor graph designed to work with expressions. It extends the capabilities of a standard factor graph by allowing factors created from [GTSAM expressions](../../../doc/expressions.md), that implement automatic differentiation. It creates [ExpressionFactors](ExpressionFactor.ipynb).\n",
|
||||||
"\n",
|
"\n",
|
||||||
"## Key Features\n",
|
"### Adding Expression Factors\n",
|
||||||
"\n",
|
"\n",
|
||||||
"- **Expression Handling**: The class allows for the creation and manipulation of factors that are expressed symbolically. This can be advantageous in scenarios where the relationships between variables are best described using mathematical expressions.\n",
|
"use **addExpressionFactor**: This method allows the user to add a new factor to the graph based on a symbolic expression. The expression defines the relationship between the variables involved in the factor.\n",
|
||||||
"\n",
|
"```c++\n",
|
||||||
"- **Automatic Differentiation**: By leveraging expressions, the class supports automatic differentiation, which is crucial for optimizing complex systems where derivatives are needed.\n",
|
" template<typename T>\n",
|
||||||
"\n",
|
" void addExpressionFactor(const Expression<T>& h, const T& z,\n",
|
||||||
"- **Integration with GTSAM**: As part of the GTSAM library, `ExpressionFactorGraph` seamlessly integrates with other components, allowing for robust and efficient factor graph optimization.\n",
|
" const SharedNoiseModel& R) {\n",
|
||||||
"\n",
|
" using F = ExpressionFactor<T>;\n",
|
||||||
"## Main Methods\n",
|
" push_back(std::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));\n",
|
||||||
"\n",
|
" }\n",
|
||||||
"### Adding Factors\n",
|
"```"
|
||||||
"\n",
|
|
||||||
"- **addExpressionFactor**: This method allows the user to add a new factor to the graph based on a symbolic expression. The expression defines the relationship between the variables involved in the factor.\n",
|
|
||||||
"\n",
|
|
||||||
"### Graph Operations\n",
|
|
||||||
"\n",
|
|
||||||
"- **update**: This method updates the factor graph with new information. It recalculates the necessary components to ensure that the graph remains consistent with the added expressions.\n",
|
|
||||||
"\n",
|
|
||||||
"- **linearize**: Converts the expression-based factor graph into a linear factor graph. This is a crucial step for optimization, as many algorithms operate on linear approximations of the problem.\n",
|
|
||||||
"\n",
|
|
||||||
"### Optimization\n",
|
|
||||||
"\n",
|
|
||||||
"- **optimize**: This method runs the optimization process on the factor graph. It uses the symbolic expressions to guide the optimization, ensuring that the solution respects the relationships defined by the expressions.\n",
|
|
||||||
"\n",
|
|
||||||
"## Mathematical Foundations\n",
|
|
||||||
"\n",
|
|
||||||
"The `ExpressionFactorGraph` leverages several mathematical concepts to perform its functions:\n",
|
|
||||||
"\n",
|
|
||||||
"- **Factor Graphs**: A factor graph is a bipartite graph representing the factorization of a function. In the context of GTSAM, it is used to represent the joint probability distribution of a set of variables.\n",
|
|
||||||
"\n",
|
|
||||||
"- **Expressions**: Symbolic expressions are used to define the relationships between variables. These expressions can be differentiated and manipulated symbolically, providing flexibility and power in modeling complex systems.\n",
|
|
||||||
"\n",
|
|
||||||
"- **Automatic Differentiation**: This technique is used to compute derivatives of functions defined by expressions. It is essential for optimization algorithms that require gradient information.\n",
|
|
||||||
"\n",
|
|
||||||
"## Conclusion\n",
|
|
||||||
"\n",
|
|
||||||
"The `ExpressionFactorGraph` class is a powerful tool within the GTSAM library, offering advanced capabilities for working with symbolic expressions in factor graphs. Its integration of automatic differentiation and symbolic computation makes it particularly useful for complex optimization problems where traditional numerical methods may fall short. Users familiar with factor graphs and symbolic mathematics will find this class to be a valuable addition to their toolkit."
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"metadata": {},
|
"metadata": {
|
||||||
|
"language_info": {
|
||||||
|
"name": "python"
|
||||||
|
}
|
||||||
|
},
|
||||||
"nbformat": 4,
|
"nbformat": 4,
|
||||||
"nbformat_minor": 5
|
"nbformat_minor": 5
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -5,70 +5,75 @@
|
||||||
"id": "93869c17",
|
"id": "93869c17",
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"source": [
|
"source": [
|
||||||
"# ExtendedKalmanFilter Class Documentation\n",
|
"# ExtendedKalmanFilter\n",
|
||||||
"\n",
|
|
||||||
"*Disclaimer: This documentation was generated by AI and may require human revision for accuracy and completeness.*\n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"## Overview\n",
|
"## Overview\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The `ExtendedKalmanFilter` class in GTSAM is a robust implementation of the Extended Kalman Filter (EKF), which is a powerful tool for estimating the state of a nonlinear dynamic system. The EKF extends the capabilities of the traditional Kalman Filter by linearizing about the current mean and covariance, making it suitable for nonlinear systems.\n",
|
"The `ExtendedKalmanFilter` class in GTSAM is an implementation of the [Extended Kalman Filter (EKF)](https://en.wikipedia.org/wiki/Extended_Kalman_filter), which is a powerful tool for estimating the state of a nonlinear dynamic system.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"## Key Features\n",
|
"See also [this notebook](../../../python/gtsam/examples/easyPoint2KalmanFilter.ipynb) for the python version of the C++ example below."
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"id": "161c36eb",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"## Using the ExtendedKalmanFilter Class\n",
|
||||||
"\n",
|
"\n",
|
||||||
"- **Nonlinear State Estimation**: The EKF is designed to handle systems where the state transition and observation models are nonlinear.\n",
|
"The `ExtendedKalmanFilter` class in GTSAM provides a flexible way to implement Kalman filtering using factor graphs. Here's a step-by-step guide based on the example provided in [easyPoint2KalmanFilter.cpp](https://github.com/borglab/gtsam/blob/develop/examples/easyPoint2KalmanFilter.cpp):\n",
|
||||||
"- **Predict and Update Cycles**: The class provides mechanisms to predict the future state and update the current state estimate based on new measurements.\n",
|
|
||||||
"- **Covariance Management**: It maintains and updates the state covariance matrix, which represents the uncertainty of the state estimate.\n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"## Mathematical Foundation\n",
|
"### Steps to Use the ExtendedKalmanFilter\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The EKF operates on the principle of linearizing nonlinear functions around the current estimate. The primary equations involved in the EKF are:\n",
|
"1. **Initialize the Filter**:\n",
|
||||||
|
" - Define the initial state (e.g., position) and its covariance.\n",
|
||||||
|
" - Create a key for the initial state.\n",
|
||||||
|
" - Instantiate the `ExtendedKalmanFilter` object with the initial state and covariance.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"1. **State Prediction**:\n",
|
" ```cpp\n",
|
||||||
" $$ \\hat{x}_{k|k-1} = f(\\hat{x}_{k-1|k-1}, u_k) $$\n",
|
" Point2 x_initial(0.0, 0.0);\n",
|
||||||
" $$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$\n",
|
" SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));\n",
|
||||||
|
" Symbol x0('x', 0);\n",
|
||||||
|
" ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);\n",
|
||||||
|
" ```\n",
|
||||||
"\n",
|
"\n",
|
||||||
"2. **Measurement Update**:\n",
|
"2. Predict the Next State:\n",
|
||||||
" $$ y_k = z_k - h(\\hat{x}_{k|k-1}) $$\n",
|
|
||||||
" $$ S_k = H_k P_{k|k-1} H_k^T + R_k $$\n",
|
|
||||||
" $$ K_k = P_{k|k-1} H_k^T S_k^{-1} $$\n",
|
|
||||||
" $$ \\hat{x}_{k|k} = \\hat{x}_{k|k-1} + K_k y_k $$\n",
|
|
||||||
" $$ P_{k|k} = (I - K_k H_k) P_{k|k-1} $$\n",
|
|
||||||
"\n",
|
"\n",
|
||||||
"Where:\n",
|
" - Define the motion model using a BetweenFactor.\n",
|
||||||
"- $f$ and $h$ are the nonlinear state transition and measurement functions, respectively.\n",
|
" - Predict the next state using the predict method.\n",
|
||||||
"- $F_k$ and $H_k$ are the Jacobians of $f$ and $h$.\n",
|
" ```cpp\n",
|
||||||
"- $Q_k$ and $R_k$ are the process and measurement noise covariance matrices.\n",
|
" Symbol x1('x', 1);\n",
|
||||||
|
" Point2 difference(1, 0);\n",
|
||||||
|
" SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);\n",
|
||||||
|
" BetweenFactor<Point2> factor1(x0, x1, difference, Q);\n",
|
||||||
|
" Point2 x1_predict = ekf.predict(factor1);\n",
|
||||||
|
" ```\n",
|
||||||
"\n",
|
"\n",
|
||||||
"## Key Methods\n",
|
"3. Update the State with Measurements:\n",
|
||||||
|
" - Define the measurement model using a PriorFactor.\n",
|
||||||
|
" - Update the state using the update method.\n",
|
||||||
|
" ```cpp\n",
|
||||||
|
" Point2 z1(1.0, 0.0);\n",
|
||||||
|
" SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);\n",
|
||||||
|
" PriorFactor<Point2> factor2(x1, z1, R);\n",
|
||||||
|
" Point2 x1_update = ekf.update(factor2);\n",
|
||||||
|
" ```\n",
|
||||||
|
"4. Repeat for Subsequent Time Steps:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"### Initialization\n",
|
" - Repeat the prediction and update steps for subsequent states and measurements.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"- **Constructor**: Initializes the filter with a given initial state and covariance.\n",
|
"## Example Use Case\n",
|
||||||
|
"This example demonstrates tracking a moving 2D point using a simple linear motion model and position measurements. The ExtendedKalmanFilter class allows for flexible modeling of both the motion and measurement processes using GTSAM's factor graph framework.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"### Prediction\n",
|
"For the full implementation, see the [easyPoint2KalmanFilter.cpp](https://github.com/borglab/gtsam/blob/develop/examples/easyPoint2KalmanFilter.cpp) file.\n"
|
||||||
"\n",
|
|
||||||
"- **predict**: Advances the state estimate to the next time step using the state transition model. It computes the predicted state and updates the state covariance matrix.\n",
|
|
||||||
"\n",
|
|
||||||
"### Update\n",
|
|
||||||
"\n",
|
|
||||||
"- **update**: Incorporates a new measurement into the state estimate. It calculates the innovation, updates the state estimate, and adjusts the covariance matrix accordingly.\n",
|
|
||||||
"\n",
|
|
||||||
"### Accessors\n",
|
|
||||||
"\n",
|
|
||||||
"- **getState**: Returns the current estimated state.\n",
|
|
||||||
"- **getCovariance**: Provides the current state covariance matrix, representing the uncertainty of the estimate.\n",
|
|
||||||
"\n",
|
|
||||||
"## Usage\n",
|
|
||||||
"\n",
|
|
||||||
"The `ExtendedKalmanFilter` class is typically used in a loop where the `predict` method is called to project the state forward in time, and the `update` method is called whenever a new measurement is available. This cycle continues, refining the state estimate and reducing uncertainty over time.\n",
|
|
||||||
"\n",
|
|
||||||
"## Conclusion\n",
|
|
||||||
"\n",
|
|
||||||
"The `ExtendedKalmanFilter` class in GTSAM is a versatile tool for state estimation in nonlinear systems. By leveraging the power of linearization, it provides accurate and efficient estimation capabilities, making it suitable for a wide range of applications in robotics, navigation, and control systems."
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"metadata": {},
|
"metadata": {
|
||||||
|
"language_info": {
|
||||||
|
"name": "python"
|
||||||
|
}
|
||||||
|
},
|
||||||
"nbformat": 4,
|
"nbformat": 4,
|
||||||
"nbformat_minor": 5
|
"nbformat_minor": 5
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue