CombinedImuFactor: Add bias effect on position jacobian #874

varunagrawal · 2021-09-16T23:03:25Z

Fixes #368

TODO

Add a unit test to capture this.
Update ImuFactor.pdf with details about the jacobians for CombinedImuFactor
Add details about Integration Covariance to ImuFactor.pdf.

…r.pdf

varunagrawal · 2021-09-17T18:22:26Z

@dellaert I need some help regarding the integration covariance. Can you point me to a resource that can help explain the mathematical basis for it?

dellaert · 2021-09-17T18:31:26Z

@dellaert I need some help regarding the integration covariance. Can you point me to a resource that can help explain the mathematical basis for it?

No, @lucacarlone and @cfo did all this. The integration covariance was always a bit of a mystery to me as well ;-) Your best bet is @lucacarlone 's TR which I think you already asked for.

varunagrawal · 2021-09-17T18:56:21Z

@dellaert I need some help regarding the integration covariance. Can you point me to a resource that can help explain the mathematical basis for it?

No, @lucacarlone and @cfo did all this. The integration covariance was always a bit of a mystery to me as well ;-) Your best bet is @lucacarlone 's TR which I think you already asked for.

Gotcha! That's the only thing left to document before this PR is ready. 🙂

dellaert · 2021-09-17T19:34:29Z

gtsam/navigation/tests/testCombinedImuFactor.cpp

+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,          //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
+
+  // regression


So, I think a "regression" is not enough here. It's easy to make a mistake, so how do we know the code is correct?

That's a great point which I have been wondering about myself. ImuFactor has a similar test (which I used as a template) so the same question can be asked about that factor.

I should add sub-tests that check for these things individually.

Yeah, especially with the non-trivial changes here, we need to think about how we can test, maybe using numbers also derivatives

lucacarlone · 2021-09-17T22:26:58Z

@dellaert I need some help regarding the integration covariance. Can you point me to a resource that can help explain the mathematical basis for it?

No, @lucacarlone and @cfo did all this. The integration covariance was always a bit of a mystery to me as well ;-) Your best bet is @lucacarlone 's TR which I think you already asked for.

Gotcha! That's the only thing left to document before this PR is ready. 🙂

I shared the report, but I suspect it will not be very useful (it mostly covers the jacobians). I'm happy to write down the math if this is not super-urgent (I cannot do that before sometimes in October).

… docs

varunagrawal · 2021-09-18T13:37:01Z

@lucacarlone yes that works. We can update the document later since this PR is focused on resolving #368. 🙂

varunagrawal · 2021-09-19T14:46:56Z

@dellaert so I've spent some more time trying to test the preintegration covariance, and I've found a lot of inconsistencies.

For the same initial conditions and measurements, the ImuFactor and the CombinedImuFactor give me significantly different covariances (6 orders of magnitude).
There are some off-diagonal elements missing in the covariance computation for CombinedImuFactor. This is easy enough to add in.

Unfortunately, without understanding the math behind the preintegration covariance, I cannot be certain whether any operation is the correct one to do.

varunagrawal · 2021-09-19T15:31:32Z

This is the preintegration covariance for the ImuFactor

3.0625e-10          0          0          0          0          0          0          0          0
         0 3.0625e-10          0          0          0          0          0          0          0
         0          0 3.0625e-10          0          0          0          0          0          0
         0          0          0    2.5e-11          0          0      5e-09          0          0
         0          0          0          0    2.5e-11          0          0      5e-09          0
         0          0          0          0          0    2.5e-11          0          0      5e-09
         0          0          0      5e-09          0          0      1e-06          0          0
         0          0          0          0      5e-09          0          0      1e-06          0
         0          0          0          0          0      5e-09          0          0      1e-06

and for the same numbers, we get for CombinedImuFactor

0.01  0     0      0           0           0           0        0        0        0    0    0    0    0    0
0     0.01  0      0           0           0           0        0        0        0    0    0    0    0    0
0     0     0.01   0           0           0           0        0        0        0    0    0    0    0    0
0     0     0      2.50025e-07 0           0           0        0        0        0    0    0    0    0    0
0     0     0      0           2.50025e-07 0           0        0        0        0    0    0    0    0    0
0     0     0      0           0           2.50025e-07 0        0        0        0    0    0    0    0    0
0     0     0      0           0           0           0.010001 0        0        0    0    0    0    0    0
0     0     0      0           0           0           0        0.010001 0        0    0    0    0    0    0
0     0     0      0           0           0           0        0        0.010001 0    0    0    0    0    0
0     0     0      0           0           0           0        0        0        0.01 0    0    0    0    0
0     0     0      0           0           0           0        0        0        0    0.01 0    0    0    0
0     0     0      0           0           0           0        0        0        0    0    0.01 0    0    0
0     0     0      0           0           0           0        0        0        0    0    0    0.01 0    0
0     0     0      0           0           0           0        0        0        0    0    0    0    0.01 0
0     0     0      0           0           0           0        0        0        0    0    0    0    0    0.01

I know what is causing the difference, I just need to understand the rationale behind it.

dellaert · 2021-09-19T16:39:55Z

Unfortunately, without understanding the math behind the preintegration covariance, I cannot be certain whether any operation is the correct one to do.

So, is this (a) the manifold or (b) tangent-space formulation that has issues?
If (a), the equations in Forster17tro should describe this.
If (b), the section Noise Propagation in ImuFactor.pdf should describe this.

You should not do any operation/correction unless it is described there. If somethings is done in the code that is not described in either document, what is it?

PS It might be easiest to start with tangent-space formulation.

dellaert · 2021-09-19T16:51:56Z

Also, which unit test fails - excluding regression tests?

varunagrawal · 2021-09-19T20:52:32Z

I'm currently working explicitly in the tangent space formulation, but this is an issue in the noise propagation at the factor level so I imagine it will show even in the manifold formulation.

None of the math regarding the covariance of the CombinedImuFactor exists in ImuFactor.pdf. What's confusing is the interaction of the integration covariance with all the Jacobians which we don't have any documentation for yet.

dellaert

Need to resolve the testing issue before we can land

dellaert · 2021-09-19T21:38:17Z

gtsam/navigation/CombinedImuFactor.h

@@ -42,6 +42,8 @@ typedef ManifoldPreintegration PreintegrationType;
 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
 * conditionally independent sets in factor graphs: a unifying perspective based
 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
+ * 
+ * [3] is available in this repo as "PreintegratedIMUJacobians.pdf".


Why not say this below where [3] is defined?

dellaert · 2021-09-19T21:39:36Z

gtsam/navigation/ImuFactor.h

@@ -44,6 +44,8 @@ typedef ManifoldPreintegration PreintegrationType;
 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
 * Robotics: Science and Systems (RSS), 2015.
+ * 
+ * [3] is available in this repo as "PreintegratedIMUJacobians.pdf".


Same: say this below

dellaert · 2021-09-19T21:40:56Z

gtsam/navigation/tests/testCombinedImuFactor.cpp

+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,          //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
+
+  // regression


Yeah, especially with the non-trivial changes here, we need to think about how we can test, maybe using numbers also derivatives

…ctor

Fix Preintegration Covariance of CombinedImuFactor

varunagrawal · 2022-07-20T21:51:19Z

@lucacarlone yes last PR. Just the one file in CombinedImuFactor.cpp needs to be reviewed since that is the main change here.

lucacarlone · 2022-07-21T15:37:29Z

@varunagrawal : to be honest I'm a bit lost among these PRs. the PR says this mostly updates the pdf, but you mention CombinedImuFactor.cpp is what's important?

varunagrawal · 2022-07-21T15:42:05Z

@lucacarlone so sorry about that. This PR was supposed to initially fix a bug causing degenerate covariances but we discovered a lot of issues when going through the IMU factor PDF, which is why you saw the other PRs built on top of this.

Honestly, this PR should be good to go since the other PRs were updates to this one. Frank prefers having smaller PRs to make reviewing easier but this time it proved to be a bit more complicated.

CombinedImuFactor.cpp is definitely the only major change here. Everything else should just be a repeat of things you've already reviewed.

varunagrawal · 2022-07-21T15:44:14Z

The original PR comment is definitely outdated.

This reverts commit 2d41b01.

lucacarlone

I'm approving this, but haven't re-reviewed the math.

varunagrawal · 2022-07-26T22:17:19Z

@lucacarlone thank you! That should be fine since the math is the same from the prior PRs you reviewed and I went from math -> code + added lots of unit tests.

@dellaert just need you to unblock this PR now and we can land it!!! 😄

varunagrawal · 2022-08-10T14:33:50Z

@dellaert I undid the name change from biasAccOmegaInt to biasAccOmegaInit. Will make another PR for that once this is merged so we can land this one.

varunagrawal · 2022-08-21T13:05:57Z

@dellaert

dellaert · 2022-08-21T20:11:25Z

Thanks for isolating the name change. Will approve now.

varunagrawal added 2 commits September 16, 2021 13:03

minor improvements to CombinedImuFactor

5e735e1

account for bias on position in jacobians of CombinedImuFactor

9dbb431

varunagrawal self-assigned this Sep 16, 2021

varunagrawal added the bugfix Fixes an issue or bug label Sep 16, 2021

varunagrawal mentioned this pull request Sep 16, 2021

CombinedImuFactor with just one measurement leads to degeneracy #368

Closed

varunagrawal requested a review from dellaert September 16, 2021 23:05

varunagrawal changed the title ~~[WIP] CombinedImuFactor: Add bias effect on position jacobian~~ CombinedImuFactor: Add bias effect on position jacobian Sep 17, 2021

varunagrawal added 4 commits September 17, 2021 13:28

Add unit test for checking covariance of CombinedImuFactor

7e48962

additional comments to make understanding the code easier

b0fcd17

no need to assign negative to jacobians as they cancel out later

5656308

add details about noise propagation for CombinedImuFactor in ImuFacto…

810f973

…r.pdf

dellaert reviewed Sep 17, 2021

View reviewed changes

Add the Preintegrated IMU jacobians tech report and mention it in the…

da220dc

… docs

varunagrawal marked this pull request as ready for review September 18, 2021 13:37

dellaert requested changes Sep 19, 2021

View reviewed changes

varunagrawal added 5 commits September 19, 2021 20:59

update ImuFactor reference

a516415

Merge branch 'develop' into fix/368

61f2cf7

add test for checking covariances between ImuFactor and CombinedImuFa…

28d0393

…ctor

print statements in ImuFactor

a10c776

typedef for Vector15

65bbe6b

Merge branch 'fix/combined-imu-cov' into fix/combined-imu

f0be857

varunagrawal mentioned this pull request May 5, 2022

Fix CombinedImuFactor #882

Merged

varunagrawal added 6 commits May 6, 2022 16:45

fix test

8dbbb1f

fix test

ce7c71b

minor refactor to follow the math better

a17134d

use prior naming scheme for bias jacobians

cb75d92

update Lyx document based on Luca's review

bbd1e3f

Merge pull request #882 from borglab/fix/combined-imu

4244345

varunagrawal mentioned this pull request Jul 17, 2022

Fix Preintegration Covariance of CombinedImuFactor #879

Merged

varunagrawal added 2 commits July 20, 2022 00:49

Merge pull request #879 from borglab/fix/combined-imu-cov

c767dfa

Fix Preintegration Covariance of CombinedImuFactor

change python CI cores to 1

2d41b01

varunagrawal added 2 commits July 22, 2022 14:29

Revert "change python CI cores to 1"

8a33a5b

This reverts commit 2d41b01.

Merge branch 'develop' into fix/368

322c080

lucacarlone approved these changes Jul 26, 2022

View reviewed changes

varunagrawal requested a review from dellaert July 26, 2022 22:17

fix setter and getter for biasAccOmegaInit

ac28b0e

varunagrawal linked an issue Aug 3, 2022 that may be closed by this pull request

The role of the integration covariance #1114

Closed

varunagrawal mentioned this pull request Aug 3, 2022

The role of the integration covariance #1114

Closed

revert name change, save for another PR

103c78b

dellaert approved these changes Aug 21, 2022

View reviewed changes

varunagrawal merged commit 587678e into develop Aug 21, 2022

varunagrawal deleted the fix/368 branch August 21, 2022 20:33

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

CombinedImuFactor: Add bias effect on position jacobian #874

CombinedImuFactor: Add bias effect on position jacobian #874

varunagrawal commented Sep 16, 2021 •

edited

Loading

varunagrawal commented Sep 17, 2021

dellaert commented Sep 17, 2021

varunagrawal commented Sep 17, 2021

dellaert Sep 17, 2021

varunagrawal Sep 18, 2021 •

edited

Loading

dellaert Sep 19, 2021

lucacarlone commented Sep 17, 2021

varunagrawal commented Sep 18, 2021

varunagrawal commented Sep 19, 2021

varunagrawal commented Sep 19, 2021

dellaert commented Sep 19, 2021

dellaert commented Sep 19, 2021

varunagrawal commented Sep 19, 2021

dellaert left a comment

dellaert Sep 19, 2021

dellaert Sep 19, 2021

dellaert Sep 19, 2021

varunagrawal commented Jul 20, 2022

lucacarlone commented Jul 21, 2022

varunagrawal commented Jul 21, 2022

varunagrawal commented Jul 21, 2022

lucacarlone left a comment

varunagrawal commented Jul 26, 2022

varunagrawal commented Aug 10, 2022

varunagrawal commented Aug 21, 2022

dellaert commented Aug 21, 2022

CombinedImuFactor: Add bias effect on position jacobian #874

CombinedImuFactor: Add bias effect on position jacobian #874

Conversation

varunagrawal commented Sep 16, 2021 • edited Loading

varunagrawal commented Sep 17, 2021

dellaert commented Sep 17, 2021

varunagrawal commented Sep 17, 2021

dellaert Sep 17, 2021

Choose a reason for hiding this comment

varunagrawal Sep 18, 2021 • edited Loading

Choose a reason for hiding this comment

dellaert Sep 19, 2021

Choose a reason for hiding this comment

lucacarlone commented Sep 17, 2021

varunagrawal commented Sep 18, 2021

varunagrawal commented Sep 19, 2021

varunagrawal commented Sep 19, 2021

dellaert commented Sep 19, 2021

dellaert commented Sep 19, 2021

varunagrawal commented Sep 19, 2021

dellaert left a comment

Choose a reason for hiding this comment

dellaert Sep 19, 2021

Choose a reason for hiding this comment

dellaert Sep 19, 2021

Choose a reason for hiding this comment

dellaert Sep 19, 2021

Choose a reason for hiding this comment

varunagrawal commented Jul 20, 2022

lucacarlone commented Jul 21, 2022

varunagrawal commented Jul 21, 2022

varunagrawal commented Jul 21, 2022

lucacarlone left a comment

Choose a reason for hiding this comment

varunagrawal commented Jul 26, 2022

varunagrawal commented Aug 10, 2022

varunagrawal commented Aug 21, 2022

dellaert commented Aug 21, 2022

varunagrawal commented Sep 16, 2021 •

edited

Loading

varunagrawal Sep 18, 2021 •

edited

Loading