-
Notifications
You must be signed in to change notification settings - Fork 162
[ImuFactor] Add an on manifold Imu Factor #279
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.
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
[ImuFactor] Add an on manifold Imu Factor #279
Conversation
|
b58932b to
cb930c9
Compare
c75ca92 to
ac03a31
Compare
ac03a31 to
6a10ac0
Compare
bradley-solliday-skydio
left a comment
There was a problem hiding this 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.
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
| * 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. |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
|
Noting previous comments are here: bradley-solliday-skydio#48 |
aaron-skydio
left a comment
There was a problem hiding this 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
| # 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]) |
There was a problem hiding this comment.
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 :)
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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).
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
| 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; |
There was a problem hiding this comment.
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
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
6a10ac0 to
d1c3e65
Compare
164c5e3 to
d7d5a05
Compare
91255e9 to
0fe0e7e
Compare
aaron-skydio
left a comment
There was a problem hiding this 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
| * 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 |
There was a problem hiding this comment.
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?)
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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).
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
0fe0e7e to
e311bc4
Compare
7970c6f to
63a4d23
Compare
| * 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 |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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"
There was a problem hiding this comment.
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.
symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
Outdated
Show resolved
Hide resolved
| * 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 |
There was a problem hiding this comment.
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
| * accel_cov is the covariance of the accelerometer measurement | ||
| * gyro_cov is the covariance of the gyroscope measurement |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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)
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
fa8474d to
9c6f3d9
Compare
aaron-skydio
left a comment
There was a problem hiding this 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
b75e14b to
50a2f1d
Compare
aaron-skydio
left a comment
There was a problem hiding this 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
| "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", |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
|
Still waiting on this one: https://github.com/symforce-org/symforce/pull/279/files#r1088224485 |
111652d to
615a9a3
Compare
aaron-skydio
left a comment
There was a problem hiding this 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, |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
| 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. | ||
| """ |
There was a problem hiding this comment.
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
615a9a3 to
deda3fc
Compare
Unit testing is left to a follow up commit.
The core use-facing api is found in
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 ofparameters that are constant and unique to each factor).
A python version of the
ImuFactorclass will come in a subsequentcommit.
Topic: on_manifold_imu_factor