IMU Preintegration #160

hayk-skydio · 2022-06-05T12:11:11Z

Add a symforce.slam.imu_preintegration module that provides a symbolic implementation of tangent preintegration, and generates Python and C++ runtime functions for the main update function. Also adds a lightweight Python wrapper class to maintain state and tests against GTSAM.

hayk-skydio · 2022-06-05T12:14:23Z

symforce/codegen/codegen_util.py

+                formatted_symbols = []
+                for j in range(value.shape[1]):
+                    for i in range(value.shape[0]):
+                        formatted_symbols.append(sm.Symbol(f"{key}[{i}, {j}]"))


@aaron-skydio @bradley-solliday-skydio I think there's a bug here that we should discuss and work out

I may have addressed it here: dc80387

I'm not sure if this breaks anything with Python codegen yet. There's a decision in play here whether 1D geo vectors get generated as lists, 1D numpy arrays, or 2D numpy arrays.

Yeah cc @nathan-skydio too, I don't think we want to generate them as lists but could see either 1d or 2d numpy arrays

Maybe generating 2D arrays makes the most sense given that a geo.Matrix always has the number of rows/cols defined, so I'm not sure a 1D geo.Matrix really exists (i.e. I think geo.V3.shape is (3,1), so we might want my_generated_numpy_vec3.shape to be (3,1) instead of (3,)). I don't feel super strongly one way or the other though

I think I agree with that. However, in javascript it will be more awkward because it'll be an Array of Arrays or something like that, where if you really just wanted scalars and vectors it adds unnecessary complexity

With dc80387 we can do this easily per language though right? I think it makes sense for python to use all-2d numpy arrays, since that's easy, but for JS to use 1D arrays for column vectors

Yes we can do 2D numpy arrays for python and use 1D arrays in JS. Just slightly odd to have different apis like that

Personally I lean towards just making things 2d everywhere if we want uniformity

Yeah I'm a bit torn on the uniformity vs idiomatic use. I wonder if we might just want to introduce a dependency like https://github.com/nicolaspanel/numjs

hayk-skydio · 2022-06-05T12:14:44Z

symforce/slam/imu_preintegration/preintegrated_imu.py

+            accel_cov:
+            gyro_cov:
+            accel_bias:
+            gyro_bias:


hayk-skydio · 2022-06-05T12:15:22Z

test/symforce_preintegrated_imu_test.py

+            import gtsam
+        except ImportError:
+            logger.info("Skipping test, GTSAM not installed.")
+            return


@aaron-skydio curious on thoughts here

Yeah I think this makes sense to skip if gtsam isn't installed, should make sure this runs in CI (where we install gtsam so we can build the benchmarks, cc @bradley-solliday-skydio ). Should probably also use unittest.skip like we do here for numba

hayk-skydio · 2022-06-05T12:41:08Z

Another thing to discuss @aaron-skydio is that this puts the python generated code into sym.factors. This makes sense to me and matches C++, but I doubt we want it in the symforce-sym package, and also it makes it perhaps so you have to pip install -e . before you can import it (ie the build is required to iterate)

aaron-skydio · 2022-06-06T21:52:04Z

I think it seems reasonable to put the generated python factors into symforce-sym, since we'll presumably do the same for C++ generated factor headers. Rerunning pip install -e . to update symforce-sym is something we should additionally fix though

varunagrawal · 2022-08-07T23:39:15Z

symforce/slam/imu_preintegration/preintegrated_imu.py

+from sym.factors.imu_preintegration_update import imu_preintegration_update
+
+
+class PreintegratedImu:


I have 2 quick questions if I may:

Can we keep the same naming convention as GTSAM? That should make porting code over to symforce much easier.

I've made a bunch of improvements to the ImuFactor, CombinedImuFactor and other parts related to these classes. I was wondering if you're using GTSAM as a code reference or reimplementing this directly from the math? Goes without saying that I am more than happy to provide reviews/help to land this feature. I am considering moving all my PhD work to using Symforce for the long term. :)

@varunagrawal have the updates you mention landed in gtsam develop?

Yes they have!

@varunagrawal Oh great. I am nose down on getting this port going and comparing with current gtsam develop. Perhaps I can ping you with a question or two. =)

Absolutely! More than happy to help and see this feature landed in Symforce.

Glad y'all are interested :)

We're also currently pushing on getting this usable as a factor in the optimizer and getting it merged, it might be worth coordinating and sharing the work (@bradley-solliday-skydio is working on this right now)

@bradley-solliday-skydio i would love to discuss. I am working through unit testing the tangent pre-integration piece against gtsam, and finding some differences. Lmk.

@varunagrawal I pinged you on LinkedIn

any update on this feature?

Hey Hans! @bradley-solliday-skydio is actively working on finishing this up and we should be able to merge it soon

Add a symforce.slam.imu_preintegration module that provides a symbolic implementation of tangent preintegration, and generates Python and C++ runtime functions for the main update function. Also adds a lightweight Python wrapper class to maintain state and tests against GTSAM. GitOrigin-RevId: 320a1690e2bef50bff14001408552976bfe2ee5c

aaron-skydio · 2023-03-10T23:50:29Z

This was moved to #279, which is now merged - further work on this should be new PR(s)

hayk-skydio requested review from aaron-skydio and dominic-skydio June 5, 2022 12:13

hayk-skydio commented Jun 5, 2022

View reviewed changes

symforce/slam/imu_preintegration/preintegrated_imu.py

accel_cov:

gyro_cov:

accel_bias:

gyro_bias:

Copy link

Contributor Author

hayk-skydio Jun 5, 2022

Choose a reason for hiding this comment

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

populate

hayk-skydio commented Jun 5, 2022

View reviewed changes

hayk-skydio mentioned this pull request Jun 5, 2022

Centralize code for creating new code generation backends #158

Closed

varunagrawal reviewed Aug 8, 2022

View reviewed changes

bradley-solliday-skydio force-pushed the imu_preintegration branch 4 times, most recently from 96d9c95 to f02ba16 Compare September 22, 2022 02:25

bradley-solliday-skydio changed the base branch from main to versioned_dev_requirements September 22, 2022 02:26

bradley-solliday-skydio force-pushed the imu_preintegration branch from f02ba16 to 6efbfb6 Compare September 22, 2022 22:49

bradley-solliday-skydio changed the base branch from versioned_dev_requirements to main September 22, 2022 22:51

bradley-solliday-skydio force-pushed the imu_preintegration branch 2 times, most recently from 8972fd4 to aaabed1 Compare September 24, 2022 03:04

bradley-solliday-skydio force-pushed the imu_preintegration branch from aaabed1 to d87f7b1 Compare November 9, 2022 00:00

aaron-skydio closed this Mar 10, 2023

aaron-skydio deleted the imu_preintegration branch March 10, 2023 23:50

		from sym.factors.imu_preintegration_update import imu_preintegration_update


		class PreintegratedImu:

IMU Preintegration #160

IMU Preintegration #160

Uh oh!

Conversation

hayk-skydio commented Jun 5, 2022

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

hayk-skydio commented Jun 5, 2022

Uh oh!

aaron-skydio commented Jun 6, 2022

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

aaron-skydio commented Mar 10, 2023

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

7 participants