12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Addressed PR feedback: applied suggested improvements

release/4.3a0
mkielo3 2025-02-20 22:12:46 +00:00
parent 9e676b215e
commit 53b1ce3885
2 changed files with 44 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -33,6 +33,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using symbol_shorthand::X;
int main() { int main() {
@ -54,18 +55,20 @@ int main() {
// i.e., we should get 0,0 0,1 0,2 if there is no noise // i.e., we should get 0,0 0,1 0,2 if there is no noise
// Create new state variable // Create new state variable
Symbol x0('x',0); Key x0 = X(0);
ordering->push_back(x0); ordering->push_back(x0);
// Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0) // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
// This is equivalent to x_0 and P_0 // This is equivalent to x_0 and P_0
Point2 x_initial(0,0); Point2 x_initial(0,0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished()); SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 0.1);
// Create a JacobianFactor directly - this represents the prior constraint on x0 // Create a JacobianFactor directly - this represents the prior constraint on x0
JacobianFactor::shared_ptr factor1( auto factor1 = std::make_shared<JacobianFactor>(
new JacobianFactor(x0, P_initial->R(), Vector::Zero(2), x0,
noiseModel::Unit::Create(2))); P_initial->R(),
Vector::Zero(2),
noiseModel::Unit::Create(2));
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x0, x_initial); linearizationPoints.insert(x0, x_initial);
@ -91,11 +94,11 @@ int main() {
// so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t} // so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t}
// = (F - I)*x_{t} + B*u_{t} // = (F - I)*x_{t} + B*u_{t}
// = B*u_{t} (for our example) // = B*u_{t} (for our example)
Symbol x1('x',1); Key x1 = X(1);
ordering->push_back(x1); ordering->push_back(x1);
Point2 difference(1,0); Point2 difference(1,0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished()); SharedDiagonal Q = noiseModel::Isotropic::Sigma(2, 0.1);
BetweenFactor<Point2> factor2(x0, x1, difference, Q); BetweenFactor<Point2> factor2(x0, x1, difference, Q);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x1, x_initial); linearizationPoints.insert(x1, x_initial);
@ -119,7 +122,7 @@ int main() {
// Extract the current estimate of x1,P1 from the Bayes Network // Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = bayesNet->optimize(); VectorValues result = bayesNet->optimize();
Point2 x1_predict = linearizationPoints.at<Point2>(x1) + Point2(result[x1]); Point2 x1_predict = linearizationPoints.at<Point2>(x1) + result[x1];
traits<Point2>::Print(x1_predict, "X1 Predict"); traits<Point2>::Print(x1_predict, "X1 Predict");
// Update the new linearization point to the new estimate // Update the new linearization point to the new estimate
@ -143,11 +146,11 @@ int main() {
// -> b'' = b' - F(dx1' - dx1'') // -> b'' = b' - F(dx1' - dx1'')
// = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2 // = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2
// = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2 // = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2
JacobianFactor::shared_ptr newPrior(new JacobianFactor( auto newPrior = std::make_shared<JacobianFactor>(
x1, x1,
x1Conditional->R(), x1Conditional->R(),
x1Conditional->d() - x1Conditional->R() * result[x1], x1Conditional->d() - x1Conditional->R() * result[x1],
x1Conditional->get_model())); x1Conditional->get_model());
// Ensure correct number of rows, that there is one variable, and that variable is x1 // Ensure correct number of rows, that there is one variable, and that variable is x1
assert(newPrior->rows() == x1Conditional->R().rows()); assert(newPrior->rows() == x1Conditional->R().rows());
@ -183,7 +186,7 @@ int main() {
// = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
// This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R. // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
Point2 z1(1.0, 0.0); Point2 z1(1.0, 0.0);
SharedDiagonal R1 = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.25, 0.25).finished()); SharedDiagonal R1 = noiseModel::Isotropic::Sigma(2, 0.25);
PriorFactor<Point2> factor4(x1, z1, R1); PriorFactor<Point2> factor4(x1, z1, R1);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor4.linearize(linearizationPoints)); linearFactorGraph->push_back(factor4.linearize(linearizationPoints));
@ -201,7 +204,7 @@ int main() {
// Extract the current estimate of x1 from the Bayes Network // Extract the current estimate of x1 from the Bayes Network
VectorValues updatedResult = updatedBayesNet->optimize(); VectorValues updatedResult = updatedBayesNet->optimize();
Point2 x1_update = linearizationPoints.at<Point2>(x1) + Point2(updatedResult[x1]); Point2 x1_update = linearizationPoints.at<Point2>(x1) + updatedResult[x1];
traits<Point2>::Print(x1_update, "X1 Update"); traits<Point2>::Print(x1_update, "X1 Update");
// Update the linearization point to the new estimate // Update the linearization point to the new estimate
@ -219,11 +222,11 @@ int main() {
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
// The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0, // The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0,
// the first key in the next iteration // the first key in the next iteration
JacobianFactor::shared_ptr updatedPrior(new JacobianFactor( auto updatedPrior = std::make_shared<JacobianFactor>(
x1, x1,
updatedConditional->R(), updatedConditional->R(),
updatedConditional->d() - updatedConditional->R() * updatedResult[x1], updatedConditional->d() - updatedConditional->R() * updatedResult[x1],
updatedConditional->get_model())); updatedConditional->get_model());
// Ensure correct number of rows, that there is one variable, and that variable is x1 // Ensure correct number of rows, that there is one variable, and that variable is x1
assert(updatedPrior->rows() == updatedConditional->R().rows()); assert(updatedPrior->rows() == updatedConditional->R().rows());
@ -233,7 +236,7 @@ int main() {
linearFactorGraph->push_back(updatedPrior); linearFactorGraph->push_back(updatedPrior);
// Create a key for the new state // Create a key for the new state
Symbol x2('x',2); Key x2 = X(2);
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = Ordering::shared_ptr(new Ordering);
@ -242,7 +245,7 @@ int main() {
// Create a nonlinear factor describing the motion model (moving right again) // Create a nonlinear factor describing the motion model (moving right again)
Point2 difference2(1,0); Point2 difference2(1,0);
SharedDiagonal Q2 = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished()); SharedDiagonal Q2 = noiseModel::Isotropic::Sigma(2, 0.1);
BetweenFactor<Point2> factor6(x1, x2, difference2, Q2); BetweenFactor<Point2> factor6(x1, x2, difference2, Q2);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
@ -255,7 +258,7 @@ int main() {
// Extract the predicted state // Extract the predicted state
VectorValues prediction2Result = predictionBayesNet2->optimize(); VectorValues prediction2Result = predictionBayesNet2->optimize();
Point2 x2_predict = linearizationPoints.at<Point2>(x2) + Point2(prediction2Result[x2]); Point2 x2_predict = linearizationPoints.at<Point2>(x2) + prediction2Result[x2];
traits<Point2>::Print(x2_predict, "X2 Predict"); traits<Point2>::Print(x2_predict, "X2 Predict");
// Update the linearization point to the new estimate // Update the linearization point to the new estimate
@ -266,11 +269,11 @@ int main() {
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
JacobianFactor::shared_ptr prior2(new JacobianFactor( auto prior2 = std::make_shared<JacobianFactor>(
x2, x2,
x2Conditional->R(), x2Conditional->R(),
x2Conditional->d() - x2Conditional->R() * prediction2Result[x2], x2Conditional->d() - x2Conditional->R() * prediction2Result[x2],
x2Conditional->get_model())); x2Conditional->get_model());
assert(prior2->rows() == x2Conditional->R().rows()); assert(prior2->rows() == x2Conditional->R().rows());
assert(prior2->size() == 1); assert(prior2->size() == 1);
@ -303,7 +306,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network // Extract the current estimate of x2 from the Bayes Network
VectorValues updatedResult2 = updatedBayesNet2->optimize(); VectorValues updatedResult2 = updatedBayesNet2->optimize();
Point2 x2_update = linearizationPoints.at<Point2>(x2) + Point2(updatedResult2[x2]); Point2 x2_update = linearizationPoints.at<Point2>(x2) + updatedResult2[x2];
traits<Point2>::Print(x2_update, "X2 Update"); traits<Point2>::Print(x2_update, "X2 Update");
// Update the linearization point to the new estimate // Update the linearization point to the new estimate
@ -321,16 +324,16 @@ int main() {
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
Matrix updatedR2 = updatedConditional2->R(); Matrix updatedR2 = updatedConditional2->R();
Vector updatedD2 = updatedConditional2->d() - updatedR2 * updatedResult2[x2]; Vector updatedD2 = updatedConditional2->d() - updatedR2 * updatedResult2[x2];
JacobianFactor::shared_ptr updatedPrior2(new JacobianFactor( auto updatedPrior2 = std::make_shared<JacobianFactor>(
x2, x2,
updatedR2, updatedR2,
updatedD2, updatedD2,
updatedConditional2->get_model())); updatedConditional2->get_model());
linearFactorGraph->push_back(updatedPrior2); linearFactorGraph->push_back(updatedPrior2);
// Create a key for the new state // Create a key for the new state
Symbol x3('x',3); Key x3 = X(3);
// Create the desired ordering // Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering); ordering = Ordering::shared_ptr(new Ordering);
@ -339,7 +342,7 @@ int main() {
// Create a nonlinear factor describing the motion model // Create a nonlinear factor describing the motion model
Point2 difference3(1,0); Point2 difference3(1,0);
SharedDiagonal Q3 = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished()); SharedDiagonal Q3 = noiseModel::Isotropic::Sigma(2, 0.1);
BetweenFactor<Point2> factor10(x2, x3, difference3, Q3); BetweenFactor<Point2> factor10(x2, x3, difference3, Q3);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
@ -352,7 +355,7 @@ int main() {
// Extract the current estimate of x3 from the Bayes Network // Extract the current estimate of x3 from the Bayes Network
VectorValues prediction3Result = predictionBayesNet3->optimize(); VectorValues prediction3Result = predictionBayesNet3->optimize();
Point2 x3_predict = linearizationPoints.at<Point2>(x3) + Point2(prediction3Result[x3]); Point2 x3_predict = linearizationPoints.at<Point2>(x3) + prediction3Result[x3];
traits<Point2>::Print(x3_predict, "X3 Predict"); traits<Point2>::Print(x3_predict, "X3 Predict");
// Update the linearization point to the new estimate // Update the linearization point to the new estimate
@ -365,11 +368,11 @@ int main() {
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph); linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step // Convert the root conditional, P(x1) in this case, into a Prior for the next step
JacobianFactor::shared_ptr prior3(new JacobianFactor( auto prior3 = std::make_shared<JacobianFactor>(
x3, x3,
x3Conditional->R(), x3Conditional->R(),
x3Conditional->d() - x3Conditional->R() * prediction3Result[x3], x3Conditional->d() - x3Conditional->R() * prediction3Result[x3],
x3Conditional->get_model())); x3Conditional->get_model());
linearFactorGraph->push_back(prior3); linearFactorGraph->push_back(prior3);
@ -379,7 +382,7 @@ int main() {
// And update using z3 ... // And update using z3 ...
Point2 z3(3.0, 0.0); Point2 z3(3.0, 0.0);
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.25, 0.25).finished()); SharedDiagonal R3 = noiseModel::Isotropic::Sigma(2, 0.25);
PriorFactor<Point2> factor12(x3, z3, R3); PriorFactor<Point2> factor12(x3, z3, R3);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
@ -398,7 +401,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network // Extract the current estimate of x2 from the Bayes Network
VectorValues updatedResult3 = updatedBayesNet3->optimize(); VectorValues updatedResult3 = updatedBayesNet3->optimize();
Point2 x3_update = linearizationPoints.at<Point2>(x3) + Point2(updatedResult3[x3]); Point2 x3_update = linearizationPoints.at<Point2>(x3) + updatedResult3[x3];
traits<Point2>::Print(x3_update, "X3 Update"); traits<Point2>::Print(x3_update, "X3 Update");
// Update the linearization point to the new estimate // Update the linearization point to the new estimate

8
gtsam/nonlinear/nonlinear.i
View File

@ -725,5 +725,13 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
}; };
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
template<T = {gtsam::Point2, gtsam::Pose2, gtsam::Pose3}>
class ExtendedKalmanFilter {
ExtendedKalmanFilter(size_t key_initial, T x_initial, gtsam::noiseModel::Gaussian* P_initial);
T predict(const gtsam::NoiseModelFactor& motionFactor);
T update(const gtsam::NoiseModelFactor& measurementFactor);
gtsam::JacobianFactor* Density() const;
};
} // namespace gtsam } // namespace gtsam