allwpilib
allwpilib copied to clipboard
wpilibsuite
[wpimath] Simplify Vision Updates for Pose Estimation
This PR primarily changes the vision update algorithm so that we no longer need to replay kinematic measurements as a final step. Instead, we can know precisely what motion in SE(2) occurs as a result of these updates by calculating the Transform between the odometry estimate from when the vision estimate occurred, and the latest odometry estimate. A previous iteration of this routine used a Twist between odometry estimates instead of a transform, but simply using a transform accomplishes the same result.
This avoids any potential problems with interpolating odometry measurements, as we can more precisely interpolate between poses using twists.
Additionally, this PR reduces the number of instances of Pose Estimator tests from 3 to 1. The only difference between the current wrappers around PoseEstimator are the kinematic primitives used and the odometry class. Those classes each have their own set of tests, so it is sufficient to test how Pose Estimation integrates vision measurements with a simplified odometry system, removing the need to simulate any one drivetrain within the test.
Still needs c++.
I tried making that utility class final and giving it a private constructor, but then the linters complained about "a class that's not instantiatable not having static methods".
It is a bit odd for the class to only hold other classes, but it doesn't matter much.
A C++ version is up on jlmcmchl#1, by the way. (I'm not sure if just mentioning this PR from that PR would bring it to people's attention)
Thanks for commenting - I didn't even know the PR existed until yesterday. Tests look good and convince me that it's implemented right.
@jlmcmchl Would you be able to merge main into your branch? (The merge conflicts seem to be from #6426, which also added PoseEstimator.sampleAt(). Pick one version of sampleAt() to go with, the added tests that automatically got moved to PoseEstimatorTest should probably be changed to use SE2Kinematics, and I suspect some of the edge cases in the sampleAt() test will have different behavior with the new implementation.)
Also, though I like both changes, I'm still iffy about bundling the test simplification with the implementation simplification, especially since the test simplification seems to have reduced accuracy. (Also, the inconsistency between C++ and Java tests should probably be resolved)