search

wpilibsuite / allwpilib

Official Repository of WPILibJ and WPILibC
https://wpilib.org/
Other
1.06k stars 614 forks source link

Generalize the odometry and pose estimator classes to 3D #4933

Open calcmogul opened 1 year ago

calcmogul commented 1 year ago

The tilting mechanism on the 2023 field means robots are not always on the x-y plane. We should be able to generalize the odometry and pose estimator classes to handle this case though.

We can make the internals use 3D poses instead of just 2D and use Pose3d.exp(). The encoder data would go in Twist3d.dx and Twist3d.dy (Twist3d.dz is zero), and the Twist3d’s orientation change would be the change in an IMU’s full 3D orientation.

We can keep the 2D gyro measurement and vision pose overloads. Adding 3D overloads for update(), addVisionMeasurement() and the constructor with standard deviations shouldn't be an API breaking change.

spacey-sooty commented 1 month ago

Should we just 0 values in 2d overloads?

calcmogul commented 1 month ago

The options are:

  1. Make the existing pose estimator use 3D internally and expose method overloads for 2D and 3D.
  2. Make separate pose estimator classes for 2D and 3D.

Option 1 is easier, but has a performance cost for those who only care about 2D. With that said, any games with a driving surface that isn't entirely flat will require the 3D version for accuracy reasons. I'd use 3D all the time with the IMU's Rotation3d in my own robot code.

We should profile the 3D version afterwards. Ideally it's fast enough that we can use 3D everywhere.