Fix ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport

[bhall-ctre]

Fixes #8284.

If we have vision updates at the time of the Reset* call, we can correct the translation/rotation of the new odometry pose by adding a new vision update where:

• ResetTranslation: the translation is hard-coded to the new translation, and the rotation components are set to those of the latest vision update (prior to clearing the map).
• ResetRotation: the rotation is hard-coded to the new rotation, and the translation components are set to those of the latest vision update (prior to clearing the map).

[KangarooKoala]

Why go with this route instead of calling resetPose()? I feel like translation and rotation are so interconnected that it makes sense for reseting rotation to be expressed as reseting the entire pose with a new rotation (and likewise with translation).

[bhall-ctre]

I did consider just calling resetPose in both cases (as demonstrated in the issue tracker). However, it was not clear if it was acceptable to reset the odometry object's translation/rotation component to that of the vision-compensated pose rather than leave it untouched.

As this PR is written now, it remains possible to pull out both the PoseEstimator pose and the Odometry pose and have resetTranslation/resetRotation only reset the translation/rotation of each. That also leaves open the possibility of using a single PoseEstimator instance to pull out both uncompensated odometry and the vision-compensated pose estimate (which was technically possible last year if you re-implemented the SwerveDrivePoseEstimator subclass to include a public Odometry instance).

[KangarooKoala]

as demonstrated in the issue tracker

Whoops, my bad! I saw that note earlier but then completely forgot when revisiting this PR.

That also leaves open the possibility of using a single PoseEstimator instance to pull out both uncompensated odometry and the vision-compensated pose estimate

Good point- That was something I had noted to myself when doing the InterpolationRecord -> VisionUpdate refactor, and I had considered exposing a getOdometryOnlyPose() method or similar (but never got around to analyzing whether it was a feature of the implementation that was worth exposing due to the constraint on the implementation it adds). Reseting rotation to gyro angle is also a good example of when you might want to reset the rotation of both without changing either translation.

At the same time, I don't like the asymmetry this introduces between resetRotation()/resetTranslation() and resetPose()/resetPosition(). After all, resetRotation(), resetTranslation(), and resetPose() were originally requested as a convenience wrapper over resetPosition() to avoid having to reread the encoder and gyro values. As such, a reasonable choice of the resetRotation() and resetTranslation() semantics is to behave like a wrapper over resetPose().

To be honest, if the implementation complexity was not a concern, I do favor the "separately reset the odometry and pose estimator rotation" semantics- I'm just having trouble justifying the additional implementation complexity for a semantic difference that isn't even exposed to users. (Also, adding complexity for the sake of purity feels shaky- I'm not saying you're making that argument, it was just another potential reason I thought of while writing this.)

Maybe we use resetPose() right now, with the vision update implementation being part of exposing the odometry-only pose when we decide if the implementation constraint is worth it?

[bhall-ctre]

At the same time, I don't like the asymmetry this introduces between resetRotation()/resetTranslation() and resetPose()/resetPosition()

I had a similar thought at first, but if by asymmetry you mean how the former now keep a VisionUpdate around and the latter don't, I did realize that we could make them symmetric by pushing a VisionUpdate where both poses are equivalent to the new reset pose. However, I believe that would have no impact on behavior, unless this implementation actually does need something pushed to m_odometryPoseBuffer.

I'm just having trouble justifying the additional implementation complexity for a semantic difference that isn't even exposed to users. (Also, adding complexity for the sake of purity feels shaky- I'm not saying you're making that argument, it was just another potential reason I thought of while writing this.)

For what it's worth, we have been considering exposing odometry without vision compensation to our swerve users, and this PR contains the changes we were planning on rolling into our fork so we still have that option. With that said, it's understandable that WPILib may prefer to roll with the simpler change of directly calling resetPose.

[KangarooKoala]

if by asymmetry you mean how the former now keep a VisionUpdate around and the latter don't

I was thinking more on the semantics side- The former now have a difference between odometry and vision while the latter wouldn't (which amounts to effectively the same thing as whether they keep a VisionUpdate). It's an inherent part of the proposed semantics, though, so after further thought the asymmetry doesn't really amount to much of an objection.

---

After properly sitting down and looking in detail at the current PoseEstimator implementation, I have the following thoughts:

• We should not add a sample to m_odometryPoseBuffer when adding a single VisionUpdate- Instead, we just need to update the comment on m_visionUpdates.
  • When m_odometryPoseBuffer is empty and m_visionUpdates has a single sample, everything behaves as we would expect. (That is, it behaves exactly like m_odometryPoseBuffer is empty and m_visionUpdates is also empty, except that the vision update is used to compensate for the pose estimate after update()/updateWithTime().)
  • Additionally, adding a sample to m_odometryPoseBuffer would cause sampleAt() and addVisionMeasurement() to behave differently after resetRotation()/resetTranslation() compared to after resetPose()/resetPosition() in ways unrelated to maintaining an offset between between odometry and vision pose.
• Since we do not need to (and should not) add an odometry sample, the implementation is honestly still pretty trivial- It's just unfortunately verbose. As such, I now think it's reasonable to go with this implementation (and I suppose one could argue that keeping m_odometry as completely odometry-only makes things a little simpler).

Some final thoughts:

• Sorry for the back and forth- I'm still fairly new to software engineering and still fleshing out my philosophy on code design.
• I should note that I'm not an actual wpilibsuite member or full WPILib maintainer, so my opinion is not the final say.

[KangarooKoala]

Tests also need to be added.

[bhall-ctre]

For the tests, do we want an entirely new TestResetWithVision function, or do we just want to modify the TestReset function? In the case of the former, should it include all four APIs?