CombinedIMUFactor question and example

[Chris Beall]

It looks like we don't have one... I've received questions about proper usage by email:

When should one create a new CombinedPreintegratedMeasurements object with a new bias vs. just calling resetIntegration(). Calling resetIntegration doesn't have a way to provide the most recent bias estimate, so why would one ever call this instead of constructing a new CombinedPreintegratedMeasurements object? @jdong37 @cforster @lucacarlone @indelman might know the answer?

Does anyone have a simple example that demonstrates correct usage, or could someone who's used this factor make one? I can only find unit tests.

Some related issues: https://bitbucket.org/gtborg/gtsam/issues/252/bug-in-combinedimufactor-discrete-bias https://bitbucket.org/gtborg/gtsam/pull-requests/176/feature-cleanup_imufactor

[garrett]

I'm still curious about this resetIntegration() routine that doesn't reset the bias. I've changed that value to public so that I could update it after calling resetIntegration.

Along with a simple example of the CombinedIMUFactor, any chance I could get a more clear explanation as to what specifically "biasAccOmegaInt" and "integrationCovariance" parameters are? The tests don't explain them very well and when changing these parameters, I get significantly different results. I'd like to know how to compute these if possible.

[Luca Carlone]

@cbeall3 @ghemann : I went ahead and added a method to reset the integration and at the set time set a new bias (it makes sense to me, as long as the bias is only change during the reset). My fix is now in the restoreOldImuFactor brach, that should converge soon to develop, upon approval from @dellaert. Unfortunately I do not have an example of use of the imu factors within GTSAM (I can help is someone is willing to make one). The CombinedImuFactor is a more experimental version of the ImuFactor, in which one also models the covariance of the biases used during the preintegration (i.e., biasAccOmegaInt). integrationCovariance is a (usually small) covariance to model errors committed in the integration of the positions from the velocity.

[garrett]

@lucacarlone So I actually have a code snippet that tests both ImuFactor and CombinedImuFactor. It reads in a CSV (format specified at the top of the cpp) and then adds IMU measurements and GPS corrections and compares to the GPS ground truth. Both ImuFactor and CombinedImuFactor give the same results (which I'm assuming is expected), however the results don't seem to be giving me good results. Orientation will drift over time.

If you have a chance and could look to see if I'm setting these up correctly, that would be helpful. Also feel free to use this as a basis for a GTSAM ImuFactor example (if it's anywhere close to typical use).

code snippet

data snippet

[Luca Carlone]

thanks @ghemann! if @cbeall3 agrees, I'll start a branch called feature/addImuFactorExample and debug your code

[Luca Carlone]

wait: @ghemann : in your example (CSV data) the vehicle is not moving (accelerations and rotation rates are always zero). Moreover, you only get position measurements (from the GPS). In that case the gyroscope bias is not observable (essentially there is no measurement of the yaw of the vehicle), hence it is normal that you see the drift. I'm positive that if you use better data the example might already work.. please let me know if that's true.

[garrett]

@lucacarlone, the vehicle should have acceleration in the north direction (values in the second column). First column indicates which sensor type (0 is imu), then accelerometer in the north direction.

[Luca Carlone]

ok I see, I thought that was a time stamp. in that case I can prepare a branch :-)

[Chris Beall]

Awesome! Thanks Luca!

[Luca Carlone]

@ghemann @cbeall3 : I included the IMU factor example in GTSAM (examples/ImuFactorsExample) and made it work. You guys can find it in the feature/ImuFactorExample branch.

@ghemann, let me know if that works for you.

[garrett]

@lucacarlone : Ah, thanks! So the biggest issue with the CombinedFactor really just were those two parameters I was confused about, biassAccOmegaInt and integrationCovariance. These two numbers are essentially what set CombinedFactor apart from ImuFactor, right? (since they do give different results). Also, I originally was just using the CombinedFactor, but it appears that ImuFactor usually performs a bit better.

[Luca Carlone]

@ghemann : there were two issues: first, you were computing the error by summing the errors at each step; this does not make much sense and inevitably accumulates over time. The second issue (that was partially covering the first one) is that you set the integration covariance to a very large number, which means that essentially the IMU provided no information on the position (for this reason, the old position estimate was essentially identical to the GPS measurement). Regarding the factors: integrationCovariance appears in both factors and should be set to a small number (10^-8). biasAccOmegaInt is specific to the CombinedImuFactor and its value largely influences the result. In my experience, there is no clear winner between ImuFactor and CombinedImuFactor (they are not expected to produce the same result, but as far as I remember they do if you set biassAccOmegaInt to zero). thanks @ghemann ! @cbeall3 : this seems to be solved, should I start a PR?

[Frank Dellaert]

Fixed by Luca w example