CombinedImuFactor with just one measurement leads to degeneracy

[mindThomas]

Description

If only 1 measurement is integrated into the CombinedImuFactor, the position related covariance terms are all zero.
This only seems to happen for the CombinedImuFactor though. If the same single measurement is integrated into an ImuFactor all elements of the covariance matrix is filled out. No zero rows/columns.

Unfortunately, these zero rows in the covariance leads to degeneracy and the infamous 'Indeterminant linear system' error.

Steps to reproduce

1. Create a PreintegratedCombinedMeasurements
2. Integrate 1 IMU measurement into a the measurement
3. Create a CombinedImuFactor with the measurement
4. Add the factor and necessary initializations to a NonlinearFactorGraph
5. Perform optimization

In general the problem can be reproduced with the ImuFactorsExample.cpp by ensuring that only 1 measurement is integrated: preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);

Expected behavior

Even with just one measurement I would expect at the very least all diagonal element to be non-zero of the preintegrated factor covariance, just as is the case if just one measurement is integrated into the ImuFactor.

Additional information

More information can be found here: https://groups.google.com/forum/#!msg/gtsam-users/WskPzqexiNk/UwQrbBe7AAAJ

[dellaert]

@varunagrawal take a look at this?

[varunagrawal]

@mindThomas I assume by covariance term you mean the noise model of the CombinedImuFactor?

I just ran an example using your reproduction steps, and I am seeing non-zero terms in the noise model. Please let me know if I am mistaken, or better yet, can you provide me with a minimal working example?

I'll gladly fix this if I can identify the issue. 🙂

[mindThomas]

@varunagrawal Here is a minimal example:

IMU_params = gtsam.PreintegrationCombinedParams([0;0;-9.8]);

IMU_params.setAccelerometerCovariance((0.01).^2 * eye(3));
IMU_params.setGyroscopeCovariance((1.75e-4).^2 * eye(3));
IMU_params.setIntegrationCovariance((0).^2 * eye(3));
IMU_params.setOmegaCoriolis(zeros(3,1));

currentBias = gtsam.imuBias.ConstantBias(zeros(3,1), zeros(3,1));

currentSummarizedMeasurement = gtsam.PreintegratedCombinedMeasurements(IMU_params, currentBias);

accMeas = [0.1577, -0.8251, 9.6111]';
omegaMeas = [-0.0210, 0.0311, 0.0145]';
deltaT = 0.01;
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);

disp(currentSummarizedMeasurement.preintMeasCov)

Output is:

    |----  PreintROTATION ----|     |----  PreintPOSITION ----|    |----  PreintVELOCITY ----|    |------  BiasAcc ------|    |------  BiasOmega ------|
    0.0100         0         0         0         0         0         0         0         0         0         0         0         0         0         0
         0    0.0100         0         0         0         0         0         0         0         0         0         0         0         0         0
         0         0    0.0100         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0    0.0100         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0    0.0100         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0    0.0100         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0    0.0100         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0    0.0100         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0    0.0100         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0    0.0100         0         0
         0         0         0         0         0         0         0         0         0         0         0         0         0    0.0100         0
         0         0         0         0         0         0         0         0         0         0         0         0         0         0    0.0100

Conversely with the ImuFactor:

IMU_params = gtsam.PreintegrationParams([0;0;-9.8]);

IMU_params.setAccelerometerCovariance((0.01).^2 * eye(3));
IMU_params.setGyroscopeCovariance((1.75e-4).^2 * eye(3));
IMU_params.setIntegrationCovariance((0).^2 * eye(3));
IMU_params.setOmegaCoriolis(zeros(3,1));

currentBias = gtsam.imuBias.ConstantBias(zeros(3,1), zeros(3,1));

currentSummarizedMeasurement = gtsam.PreintegratedImuMeasurements(IMU_params, currentBias);

accMeas = [0.1577, -0.8251, 9.6111]';
omegaMeas = [-0.0210, 0.0311, 0.0145]';
deltaT = 0.01;
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);

disp(currentSummarizedMeasurement.preintMeasCov)

Where there is no zero rows in the measurement covariance:

    |----  PreintROTATION ----|    |----  PreintPOSITION ----|    |----  PreintVELOCITY ----|    
    0.0003         0         0         0         0         0         0         0         0
         0    0.0003         0         0         0         0         0         0         0
         0         0    0.0003         0         0         0         0         0         0
         0         0         0    0.0000         0         0    0.0050         0         0
         0         0         0         0    0.0000         0         0    0.0050         0
         0         0         0         0         0    0.0000         0         0    0.0050
         0         0         0    0.0050         0         0    1.0000         0         0
         0         0         0         0    0.0050         0         0    1.0000         0
         0         0         0         0         0    0.0050         0         0    1.0000

[tmcg0]

@mindThomas I notice that in your minimal example you're setting the integration covariance to zero. Does this bug still exist when you use a (small) non-zero positive quantity? Of course, in the ImuFactorExamples.cpp code they use eye(3)*1.0e-8.

Apologies for not confirming this myself right now, but I can check in a few days. It was just an initial thought I had when reading this issue. I certainly have a gut reaction towards never specifying actually-zero variance noise models.

[mindThomas]

@tmcg0 Theoretically speaking the integration covariance should not be necessary. The way I understand it is that you add the integration covariance to capture unmodelled errors as a result from the discrete integration (would be even worse if we weren't doing proper Lie Group integration). So if the integration is perfectly continuous there shouldn't be a need for this integration covariance.
Also the integration covariance is set to 0 in both of my minimal examples and even though the resulting position covariance is clearly computed correctly for the PreintegratedImuMeasurements case.

The result of PreintegratedCombinedMeasurements with an integration covariance of eye(3)*1.0e-8 is:

    0.0100         0         0         0         0         0         0         0         0         0         0         0         0         0         0
         0    0.0100         0         0         0         0         0         0         0         0         0         0         0         0         0
         0         0    0.0100         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0    0.0000         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0    0.0000         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0    0.0000         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0    0.0100         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0    0.0100         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0    0.0100         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0    0.0100         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0    0.0100         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0    0.0100         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0    0.0100         0         0
         0         0         0         0         0         0         0         0         0         0         0         0         0    0.0100         0
         0         0         0         0         0         0         0         0         0         0         0         0         0         0    0.0100

Now the position covariance is no longer 0, but it is still extremely small:

cond(currentSummarizedMeasurement.preintMeasCov) = 1.0001e+08

[varunagrawal]

A quick update (because it drove me nuts trying to resolve this), the output from the ImuFactor code should be multiplied by a factor of 1e-6.

[tmcg0]

@varunagrawal sorry, can you be specific as to what output you're referring to? The covariance?

@mindThomas totally should have gotten back to you a while ago. Regarding your first paragraph ("theoretically speaking...."): There's always unmodeled sources of error. Always. I feel like it should be standard practice to never use zero for the integration covariance quantity (you never know if the people who wrote the code unit tested for a zero value!). But I think your practical example in the second paragraph ("Also the integration covariance is set to 0...") does show evidence of a bug. Trivially, position covariance should be non-zero merely from integration of a (non-zero noise) accelerometer measurement.

It would be awesome if we could build a unit test for this that fails with an assert statement. I told myself I wanted to look into this and then got totally sidetracked.

[varunagrawal]

This example

IMU_params.setAccelerometerCovariance((0.01).^2 * eye(3));
IMU_params.setGyroscopeCovariance((1.75e-4).^2 * eye(3));
IMU_params.setIntegrationCovariance((0).^2 * eye(3));
IMU_params.setOmegaCoriolis(zeros(3,1));

currentBias = gtsam.imuBias.ConstantBias(zeros(3,1), zeros(3,1));

currentSummarizedMeasurement = gtsam.PreintegratedImuMeasurements(IMU_params, currentBias);

accMeas = [0.1577, -0.8251, 9.6111]';
omegaMeas = [-0.0210, 0.0311, 0.0145]';
deltaT = 0.01;
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);

disp(currentSummarizedMeasurement.preintMeasCov)

Where there is no zero rows in the measurement covariance:

    |----  PreintROTATION ----|    |----  PreintPOSITION ----|    |----  PreintVELOCITY ----|    
 1e-6 *
    0.0003         0         0         0         0         0         0         0         0
         0    0.0003         0         0         0         0         0         0         0
         0         0    0.0003         0         0         0         0         0         0
         0         0         0    0.0000         0         0    0.0050         0         0
         0         0         0         0    0.0000         0         0    0.0050         0
         0         0         0         0         0    0.0000         0         0    0.0050
         0         0         0    0.0050         0         0    1.0000         0         0
         0         0         0         0    0.0050         0         0    1.0000         0
         0         0         0         0         0    0.0050         0         0    1.0000

This issue is going to take a while since I need to redo the math for noise propagation for PreintegratedCombinedMeasurements. We already have documentation for that (ImuFactor.pdf) so having a similar doc for CombinedImuFactor would be great as a reference. Or do we have one @dellaert?

[mindThomas]

Any updates on this?

[ProfFan]

@varunagrawal

[varunagrawal]

Okay I think I figured this out! Had to go through all the math and then some but I did it.
The issue is that the handling of the jacobians wrpt translation was not being done in CombinedImuFactor. @dellaert had left a note in the code about this and I guess @mindThomas was the unfortunate soul who encountered this.

Opening a PR.

[varunagrawal]

Draft PR #874 opened. I need to update ImuFactor.pdf to account for what I've learned while solving this issue so we can verify the correctness of the PR.

[lucacarlone]

@varunagrawal @dellaert : I spoke too early: I do have an extensive write-up on the Combine IMU factor that I wrote back when I was at GT. I'll follow up shortly to share it via email.

[varunagrawal]

Awesome-sauce

[mindThomas]

@lucacarlone Is it possible that you can share this doc with the rest of us as well?

[lucacarlone]

@mindThomas : I think @varunagrawal is preparing a revised document that he will share.

[varunagrawal]

That is correct. @dellaert, @lucacarlone and I have spoken about this and I'm rewriting all the math to ensure we fix this from the inside out.