-
Notifications
You must be signed in to change notification settings - Fork 613
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
[wpimath] Simplify Vision Updates for Pose Estimation #5473
[wpimath] Simplify Vision Updates for Pose Estimation #5473
Conversation
wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java
Show resolved
Hide resolved
wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimatorTest.java
Show resolved
Hide resolved
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
Outdated
Show resolved
Hide resolved
wpimath/src/test/java/edu/wpi/first/math/estimator/PoseEstimationTestUtils.java
Outdated
Show resolved
Hide resolved
wpimath/src/test/java/edu/wpi/first/math/util/SamplingUtils.java
Outdated
Show resolved
Hide resolved
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.
There's also some slight inconsistencies between PoseEstimationTestUtil
and SamplingUtil
(non-final, doesn't make constructor private), plus the access modifiers on the nested classes are slightly odd (implicitly package-private or public would be better than specifically protected, since we don't anticipate subclassing), but that doesn't matter very much.
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
Outdated
Show resolved
Hide resolved
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. |
…or.java Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
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 |
Superseded by #6705. |
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++.