Updated elaboratePoint2KalmanFilter.cpp example and easyPoint2KF bugfix #2031

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Merged

dellaert merged 4 commits into borglab:develop from mkielo3:fix-ekf-examples

Feb 21, 2025

Contributor

mkielo3 commented Feb 20, 2025 •

edited

Loading

Fixed the elaboratePoint2KalmanFilter.cpp with:

Direct JacobianFactor creation
Updated vector creation
Updated data structure management. insert() is now push_back()

Additionally fixed minor bug in easyPoint2KalmanFilter which predicted with wrong factor.

X1 Predict[
1;
0
]
X1 Update[
1;
0
]
X2 Predict[
2;
0
]
X2 Update[
2;
0
]
X3 Predict[
3;
0
]
X3 Update[
3;
0
]


          Updated elaboratePoint2KalmanFilter.cpp example and easyPoint2KF bugfix

9e676b2

dellaert approved these changes

View reviewed changes

Member

dellaert left a comment

Awesome. I left a bunch of comments with recommendations, and it would be nice to include the output in the Pr comment, but I am approving. I trust you to implement comments where you agree with them.

examples/elaboratePoint2KalmanFilter.cpp Outdated

    
            @@ -55,17 +55,21 @@ int main() {
          
                // Create new state variable

                Symbol x0('x',0);

                ordering->insert(x0, 0);

                ordering->push_back(x0);

Member

dellaert Feb 20, 2025

Maybe use X(k) everywhere.

using symbol_shorthand::X;

Member

dellaert Feb 20, 2025

No, I mean: kill x0, x1, x2 variables. Use X(k) in-place everywhere. Slightly less efficient but good practice for copy/paste in other code.

examples/elaboratePoint2KalmanFilter.cpp Outdated

                 // 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
                 Point2 x_initial(0,0);
-                SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
-                PriorFactor<Point2> factor1(x0, x_initial, P_initial);
+                SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished());

Member

dellaert Feb 20, 2025

Use Isotropic

examples/elaboratePoint2KalmanFilter.cpp Outdated

+                 // Create a JacobianFactor directly - this represents the prior constraint on x0
+                 JacobianFactor::shared_ptr factor1(
+              	new JacobianFactor(x0, P_initial->R(), Vector::Zero(2),
+              	  noiseModel::Unit::Create(2)));

Member

dellaert Feb 20, 2025

Not sure but you might even omit noise model if Unit

examples/elaboratePoint2KalmanFilter.cpp Outdated

                 // Linearize the factor and add it to the linear factor graph
                 linearizationPoints.insert(x0, x_initial);
-                linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
+                linearFactorGraph->push_back(factor1);

Member

dellaert Feb 20, 2025

Try to use emplace_shared if you’re creating the shared pointer.

Member

dellaert Feb 20, 2025

-> emplace_shared(args)
OR
-> add(args)

so make_shared above is no longer needed.

examples/elaboratePoint2KalmanFilter.cpp Outdated

    
                Point2 difference(1,0);

                SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));

                SharedDiagonal Q = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished());

Member

dellaert Feb 20, 2025

Isotropic

examples/elaboratePoint2KalmanFilter.cpp Outdated

    
            @@ -88,14 +92,14 @@ int main() {
          
                //                                  = (F - I)*x_{t} + B*u_{t}

                //                                  = B*u_{t} (for our example)

                Symbol x1('x',1);

                ordering->insert(x1, 1);

                ordering->push_back(x1);

Member

dellaert Feb 20, 2025

X(1)

examples/elaboratePoint2KalmanFilter.cpp Outdated

+                SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.1, 0.1).finished());
+                 // Create a JacobianFactor directly - this represents the prior constraint on x0
+                 JacobianFactor::shared_ptr factor1(

Member

dellaert Feb 20, 2025

auto factor1 = std::make_shared….

examples/elaboratePoint2KalmanFilter.cpp Outdated

    
            @@ -169,10 +183,10 @@ int main() {
          
                //    = (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.

                Point2 z1(1.0, 0.0);

                SharedDiagonal R1 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));

                SharedDiagonal R1 = noiseModel::Diagonal::Sigmas((gtsam::Vector2() << 0.25, 0.25).finished());

Member

dellaert Feb 20, 2025

Isotropic

examples/elaboratePoint2KalmanFilter.cpp Outdated

-                assert(cg1->nrParents() == 0);
-                JacobianFactor tmpPrior1 = JacobianFactor(*cg1);
-                linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
+                JacobianFactor::shared_ptr updatedPrior(new JacobianFactor(

Member

dellaert Feb 20, 2025

auto make_shared is the recommended style now

examples/elaboratePoint2KalmanFilter.cpp Outdated

-                Point2 x3_update = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
-                x3_update.print("X3 Update");
+                VectorValues updatedResult3 = updatedBayesNet3->optimize();
+                Point2 x3_update = linearizationPoints.at<Point2>(x3) + Point2(updatedResult3[x3]);

Member

dellaert Feb 20, 2025

I don’t think you need the Point2()

mkielo3 added 2 commits

February 20, 2025 22:12


          Addressed PR feedback: applied suggested improvements

53b1ce3


          Reverted unintended changes to nonlinear.i

2e19450

Contributor Author

mkielo3 commented Feb 20, 2025

Thanks! I've added your suggestions. Waiting for CI checks to complete.

Contributor Author

mkielo3 commented Feb 20, 2025

Adding for reference, expected output is:

X1 Predict[
1;
0
]
X1 Update[
1;
0
]
X2 Predict[
2;
0
]
X2 Update[
2;
0
]
X3 Predict[
3;
0
]
X3 Update[
3;
0
]

Member

dellaert commented Feb 20, 2025

Two more comments. And, request: update the PR comment with the actual output in a block. Do this by edit button at the top.


          Removed make_shared & x variables

b6a1af0

Contributor Author

mkielo3 commented Feb 20, 2025

Added

Member

dellaert commented Feb 21, 2025

Very cool, I will merge after CI passes! Next, Python version? We are moving to have Python examples as notebooks, so maybe immediately make a notebook? Will require exposing in wrapper of course.

Contributor Author

mkielo3 commented Feb 21, 2025

Great, and yes- working on this!

dellaert merged commit 08147c7 into borglab:develop

36 checks passed

Member

dellaert commented Feb 21, 2025

Congrats on first PR :-)

mkielo3 deleted the fix-ekf-examples branch

February 21, 2025 14:32

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet