Skip to content

Conversation

@bradley-solliday-skydio
Copy link
Contributor

@bradley-solliday-skydio bradley-solliday-skydio commented Dec 20, 2022

Unit testing is left to a follow up commit.

The core use-facing api is found in

  • symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
  • symforce/slam/imu_preintegration/imu_factor.h

Unlike most other factors (/residual functions) in symforce, the
ImuFactor is a functor so as to store the pre-integrated values
with the function rather than in the Values. (There are a lot of
parameters that are constant and unique to each factor).

A python version of the ImuFactor class will come in a subsequent
commit.

Topic: on_manifold_imu_factor

@bradley-solliday-skydio
Copy link
Contributor Author

Reviews in this chain:
#279 [ImuFactor] Add an on manifold Imu Factor
 └#280 [ImuFactor] Unit test the on manifold ImuFactor
  └#281 [ImuFactor] tangent space Imu Factor

@bradley-solliday-skydio
Copy link
Contributor Author

bradley-solliday-skydio commented Dec 20, 2022

# head base diff date summary
0 b58932b1 1c44a943 diff Dec 19 16:48 PM 17 files changed, 12543 insertions(+)
1 cb930c9d 1c44a943 diff Dec 20 1:09 AM 7 files changed, 7430 insertions(+), 7384 deletions(-)
2 c75ca927 1c44a943 diff Dec 20 19:54 PM 10 files changed, 7587 insertions(+), 7503 deletions(-)
3 ac03a31b 1c44a943 diff Dec 21 0:49 AM 8 files changed, 5671 insertions(+), 8077 deletions(-)
4 6a10ac05 1c44a943 diff Dec 21 2:12 AM 3 files changed, 11 insertions(+), 5 deletions(-)
5 d1c3e651 1c44a943 diff Jan 3 13:30 PM 9 files changed, 7432 insertions(+), 7904 deletions(-)
6 49a9c23d 474f1bd5 diff Jan 6 9:04 AM 12 files changed, 144 insertions(+), 5942 deletions(-)
7 cd66203d 474f1bd5 diff Jan 6 9:51 AM 2 files changed, 6 insertions(+), 6 deletions(-)
8 164c5e3e 474f1bd5 diff Jan 6 12:23 PM 4 files changed, 15 insertions(+), 17 deletions(-)
9 d7d5a05f 474f1bd5 diff Jan 10 16:25 PM 8 files changed, 119 insertions(+), 19 deletions(-)
10 6e00ec4f 474f1bd5 diff Jan 11 10:57 AM 2 files changed, 17 insertions(+), 6 deletions(-)
11 a74eae62 474f1bd5 diff Jan 11 15:27 PM 5 files changed, 707 insertions(+), 692 deletions(-)
12 91255e99 474f1bd5 diff Jan 11 15:33 PM 1 file changed, 2 deletions(-)
13 0fe0e7e5 474f1bd5 diff Jan 11 18:31 PM 1 file changed, 4 insertions(+), 2 deletions(-)
14 e311bc4e 474f1bd5 diff Jan 12 11:45 AM 13 files changed, 1060 insertions(+), 1019 deletions(-)
15 7970c6f0 474f1bd5 diff Jan 12 12:42 PM 2 files changed, 4 insertions(+), 4 deletions(-)
16 63a4d237 474f1bd5 diff Jan 12 16:23 PM 1 file changed, 1 insertion(+), 1 deletion(-)
17 9630e6b6 4c368815 diff Jan 19 13:57 PM 10 files changed, 101 insertions(+), 40 deletions(-)
18 fa8474dd 4c368815 diff Jan 19 18:41 PM 1 file changed, 2 insertions(+), 2 deletions(-)
19 9c6f3d96 4c368815 diff Jan 20 10:32 AM 5 files changed, 633 insertions(+), 663 deletions(-)
20 b75e14bf 4c368815 diff Jan 25 10:12 AM 3 files changed, 85 insertions(+), 85 deletions(-)
21 50a2f1da 4c368815 diff Jan 25 21:09 PM 1 file changed, 1 insertion(+)
22 a83f3772 4c368815 diff Jan 31 17:41 PM 4 files changed, 1022 insertions(+), 65 deletions(-)
23 111652d9 4c368815 diff Jan 31 17:53 PM 2 files changed, 12 insertions(+), 7 deletions(-)
24 615a9a36 4c368815 diff Jan 31 17:57 PM 1 file changed, 13 insertions(+), 11 deletions(-)
25 deda3fc9 4c368815 diff Feb 1 14:33 PM 2 files changed, 7 insertions(+), 7 deletions(-)

@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch from b58932b to cb930c9 Compare December 20, 2022 09:09
@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch 2 times, most recently from c75ca92 to ac03a31 Compare December 21, 2022 08:50
@bradley-solliday-skydio bradley-solliday-skydio marked this pull request as ready for review December 21, 2022 10:12
@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch from ac03a31 to 6a10ac0 Compare December 21, 2022 10:12
Copy link
Contributor Author

@bradley-solliday-skydio bradley-solliday-skydio left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Had a lot of comments because there were quite a few areas I wanted feedback on.

As a note, if reviewing in the browser, the i shortcut hides (and reveals) all comments. Using it will likely make reviewing easier.

Comment on lines +22 to +45
* Based heavily on the on manifold ImuFactor found in GTSAM and on the paper:
*
* Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza,
* “IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation”,
* Robotics: Science and Systems (RSS), 2015.
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this a sufficient citation? Should I put it in other places as well?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You could also cite the original paper by Lupton.

@aaron-skydio
Copy link
Member

Noting previous comments are here: bradley-solliday-skydio#48

Copy link
Member

@aaron-skydio aaron-skydio left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Didn't get to symforce/slam/imu_preintegration/preintegrated_imu_measurements.h yet

Comment on lines +271 to +283
# Definitely correct, but not very amenable to CSE
def new_state_D(wrt_variables: T.List[T.Any]) -> sf.Matrix:
return sf.Matrix.block_matrix(
[
tangent_jacobians(new_DR, wrt_variables),
tangent_jacobians(new_Dv, wrt_variables),
tangent_jacobians(new_Dp, wrt_variables),
]
)

new_state_D_state = new_state_D([DR, Dv, Dp])
new_state_D_gyro_bias = new_state_D([gyro_bias])
new_state_D_accel_bias = new_state_D([accel_bias])
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This seems like a fantastic thing to compare against the handwritten version in a unit test :)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm. Yes. You're right. I'll come up with some way to make comparing them simple and clean.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note. I haven't actually gotten around to this. Not sure about it's priority on whether it should be punted indefinitely, reserved for a follow up PR, or added to this one (or the unit test PR that will be merged at the same time as this one).

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess I was hoping we'd do this in the unit test PR - it is a little unfortunate to just leave this here, pylint is right that constant tests are usually a bad sign. If we're not going to add this test in this PR or the unit test PR we should probably at least make this an argument to this function that defaults to the handwritten version

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The only thing that had me hesitate was sorting out how to split out the two versions (auto-derivatives vs handwritten derivatives). The thing that made me hesitate about adding an argument to the function was that it would force us to use something like functools.partial to hide it from Codegen.

But, when I thought about it a bit more, I didn't see a better alternative. Anyway, since I made it possible to generate two separate versions, I went ahead and added the unit test to the unit test PR.

Comment on lines 35 to 43
Rot3<Scalar> new_DR;
Vector3 new_Dv;
Vector3 new_Dp;
Matrix9 new_covariance;
Matrix3 new_DR_D_gyro_bias;
Matrix3 new_Dv_D_gyro_bias;
Matrix3 new_Dv_D_accel_bias;
Matrix3 new_Dp_D_gyro_bias;
Matrix3 new_Dp_D_accel_bias;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It almost seems like this set of things (maybe minus new_covariance) wants to be an LCM type generated from a python dataclass/Values, which would mean it could be one argument to this function (or the return value), and most of the copies below would just be assigning the LCM type, and some other things might be simpler as well. Just an idea, not necessarily sold on it

@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch from 6a10ac0 to d1c3e65 Compare January 3, 2023 21:30
@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch 4 times, most recently from 164c5e3 to d7d5a05 Compare January 11, 2023 00:25
@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch from 91255e9 to 0fe0e7e Compare January 12, 2023 02:31
Copy link
Member

@aaron-skydio aaron-skydio left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looking good, mostly just minor documentation stuff

Comment on lines 42 to 46
* DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat
* Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat
* Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat
* Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat
* Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should use the same ordering as we do in the update probably (probably just match the ordering in the struct everywhere?)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Huh. I agree. Not exactly sure why I did it that way here. Done.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It still seems like we have gyro then accel in some places and accel then gyro in others? Not sure if something just hasn't been pushed yet

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh. I thought you were getting at how here its the DR, Dp, Dv instead of DR, Dv, Dp. Here gyro appears first as there's no DR_D_accel_bias (as it's just zero, as naturally the orientation is independent of the acceleration).

As for the ordering of gyro and accel, I only spotted 3 locations where gyro came first: 1) The ordering of the members in PreintegratedImuMeasurements (which doesn't really affect anything, but I just changed it so accel is first there for consistency), 2) the internal symbolic function integrate_state (which I have also just changed to be accel first), and 3) in the temporary variable state_D_bias in imu_manifold_preintegration_update (which I am keeping as is because the variable names don't line up nicely if I switch the order).

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yep, (1) and (2) are what I was referring to. Fine to leave (3), although as a super-nitpick we could pick gyro first instead to match the ordering there

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The interface of imu_manifold_preintegration_update is still backwards from the rest of the code, and I think we should fix it? And then the body of ImuPreintegrator::IntegrateMeasurement which calls ImuManifoldPreintegrationUpdate

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oops. Looks like I overlooked that function. Fixed.

@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch from 0fe0e7e to e311bc4 Compare January 12, 2023 19:45
@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch 2 times, most recently from 7970c6f to 63a4d23 Compare January 13, 2023 00:24
@aaron-skydio aaron-skydio added this to the v0.8 milestone Jan 17, 2023
Comment on lines 22 to 26
* A factor for using on-manifold IMU preintegration in a SymForce optimization problem.
*
* Assumes IMU bias is constant over the preintegration window. Does not recompute the
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It would be good to include a description of we mean by "on-manifold" here

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's reasonable. Though, I've never actually seen a definition of tangent space pre-integration (I've neglected to read through the gtsam implementation details, and that's the only source I can think of as my googling prowess is not up to par). So this is what I've put and hopefully it is correct:

 * A factor for using on-manifold IMU preintegration in a SymForce optimization problem. By
 * on-manifold, it is meant that the angular velocity measurements are composed as rotations
 * rather than tangent-space approximations.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe this is correct, I might say something like "By on-manifold, we mean that the integration is performed (and integrated states are represented) on the manifold, as opposed to in the tangent space of the initial state"

Copy link

@asa asa Jan 20, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A short discussion of the benefits / differences of the on-manifold vs tangent representations would be great, if possible.

Comment on lines 42 to 46
* DR_D_gyro_bias (sf.M33): Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat
* Dp_D_accel_bias (sf.M33): Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat
* Dp_D_gyro_bias (sf.M33): Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat
* Dv_D_accel_bias (sf.M33): Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat
* Dv_D_gyro_bias (sf.M33): Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It still seems like we have gyro then accel in some places and accel then gyro in others? Not sure if something just hasn't been pushed yet

Comment on lines 47 to 48
* accel_cov is the covariance of the accelerometer measurement
* gyro_cov is the covariance of the gyroscope measurement
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also mentioned here, but we should say what units there are in - assuming the test is right it seems to me like maybe these aren't technically the covariances of the measurements, they're the imu noise densities?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, the tests are right. So the units should be (radians per time)^2 * time (or, perhaps more recognizably to some, (radians per time)^2 / frequency).

And I suppose it is also important to mention that the units are expected to be in terms of radians, not degrees.

I don't know of any simple words that clearly communicate the idea that it's the, idk, covariance density in frequency space or whatever, so I'll just append [(radians / time)^2 * time] to the ends of the lines.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Probably good to specify seconds here for the time also, since these numbers are sometimes quoted in hours - I think the square roots of these are sometimes called angle random walk / velocity random walk, but just the units should be sufficient

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I do wonder if it's more expected / intuitive for these parameters to be the covariances, or the (squared) angle/velocity random walks - maybe @chao-qu-skydio @dominic-skydio have opinions? Any idea what other libraries (e.g. GTSAM) expect?

Copy link
Contributor

@chao-qu-skydio chao-qu-skydio Jan 19, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are you talking about the names? I think xxx_cov is fine. My suggestion is to simply make it a 3x1 vector (gyro_var) instead of 3x3 matrix (gyro_cov). I've never seen a use case for a full covariance matrix on imu measurement.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Talking about the values - is it more expected for these to be the measurement covariances ((rad/s)^2) or the squared angle/velocity random walks ((rad / sqrt(Hz))^2)? I do wonder if they're the (squared) angle/velocity random walks if we should maybe name them differently, but I agree naming it xxx_cov is probably fine in either case

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

About making these diagonal, it does save a good number of ops: #279 (comment)

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

They should be the continuous-time noise density squared. The random walk is for the bias.
Most imu datasheets only report a single number for all 3 axes (or at most 3, 1 for each), so a full covariance is an overkill IMO.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well, I've changed the covariances to just be the diagonal entries, as it gives an immediate 10% op-count reduction for the integration step, and it should always be easy to go back and add a full square covariance option if we really wanted it.

@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch 3 times, most recently from fa8474d to 9c6f3d9 Compare January 20, 2023 18:32
Copy link
Member

@aaron-skydio aaron-skydio left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just one remaining comment (linking here so it doesn't get lost): #279 (comment)

Left some other comments wrapping up threads, but that's the only one that has anything I think we should definitely address, otherwise I think this is good to go! I do think we should have the PR with tests mostly ready before we actually merge this one, although I don't think we need to wait for the tests PR to be approved

@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch 2 times, most recently from b75e14b to 50a2f1d Compare January 26, 2023 05:09
Copy link
Member

@aaron-skydio aaron-skydio left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Calling out this so it doesn't get buried: https://github.com/symforce-org/symforce/pull/279/files#r1088224485

Comment on lines 26 to 27
"new_DR_D_gyro_bias",
"new_Dp_D_gyro_bias",
"new_Dp_D_accel_bias",
"new_Dv_D_gyro_bias",
"new_Dv_D_accel_bias",
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The output names here are backwards

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh oops. Forgot about that. Thanks.

@aaron-skydio
Copy link
Member

@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch 3 times, most recently from 111652d to 615a9a3 Compare February 1, 2023 01:57
Copy link
Member

@aaron-skydio aaron-skydio left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Awesome, let's go for it

# Singularity handling,
epsilon: T.Scalar,
# Configuration
use_handwritten_derivatives: bool = True,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't we want to default this to false?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well, I was thinking that it made sense to use the handwritten version because the two versions should be the same (in terms of correctness), but the handwritten version is faster. So, as a signaling mechanism as to which version you should use, I thought it made sense to default to the faster one.

But, now that I think about it, there's really no need to give a default value. I don't think there's any harm though for me to independently want to change it.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I somehow read this option as the inverse, I thought this was defaulting to automatic derivatives. We should default to using the handwritten derivatives like this already is since that's what we think you should use

Comment on lines +43 to +47
docstring="""
Alternative to ImuManifoldPreintegrationUpdate that uses auto-derivatives. Exists only to
help verify correctness of ImuManifoldPreintegrationUpdate. Should have the same output.
Since this function is more expensive, there is no reason to use it instead.
"""
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No need to change anything here, but textwrap.dedent is your friend

Unit testing is left to a follow up commit.

The core use-facing api is found in
- symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
- symforce/slam/imu_preintegration/imu_factor.h

Unlike most other factors (/residual functions) in symforce, the
ImuFactor is a functor so as to store the pre-integrated values
with the function rather than in the `Values`. (There are a lot of
parameters that are constant and unique to each factor).

A python version of the `ImuFactor` class will come in a subsequent
commit.

Topic: on_manifold_imu_factor
@bradley-solliday-skydio bradley-solliday-skydio force-pushed the bradley.solliday/revup/main/on_manifold_imu_factor branch from 615a9a3 to deda3fc Compare February 1, 2023 22:33
@aaron-skydio aaron-skydio mentioned this pull request Mar 10, 2023
@aaron-skydio aaron-skydio deleted the bradley.solliday/revup/main/on_manifold_imu_factor branch March 11, 2023 01:20
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

5 participants