Describe the bug

Both of those APIs clear the vision buffer and sample the pose from the odometry. As a result, any vision adjustments to the other component are lost, causing the pose estimate to teleport elsewhere based on the pure (pre-vision) odometry data.

For example, if at robot startup the pose is at zero, and vision corrects it to be (3 m, 2 m, pi rad), then calling resetRotation(Rotation2d.k180deg) resets the pose to the odometry-reported (0 m, 0 m, pi rad) until vision can correct for it again.

To Reproduce
Steps to reproduce the behavior:

1. Construct a SwerveDrivePoseEstimator poseEst
2. In a periodic function, call poseEst.addVisionMeasurement(new Pose2d(3.0, 2.0, Rotation2d.k180deg), Timer.getTimestamp());
3. Bind poseEst.resetRotation(Rotation2d.kCCW_90deg) to a joystick button
4. Observe how pressing the button causes the pose estimate to teleport to a translation of (0, 0)
5. Bind poseEst.resetTranslation(Translation2d.kZero) to another joystick button
6. Observe how pressing the button causes the pose estimate to teleport to a rotation of 90 deg (from the previous resetRotation)

Expected behavior
For resetRotation, the translation from the most recent pose estimate (post-vision) should be preserved.
For resetTranslation, the rotation from the most recent pose estimate (post-vision) should be preserved.

Desktop:

• OS: Windows 11
• Project Information:
  WPILib Information:
  Project Version: 2025.3.2
  WPILib Extension Version: 2025.3.2
  Vendor Libraries:
  CTRE-Phoenix (v6) (25.4.0)
  WPILib-New-Commands (1.0.0)

Additional context
One possible solution would be to make the pose estimator resetTranslation/resetRotation call resetPose(...) with a new Pose2d constructed with the provided translation/rotation and the current pose estimate's getRotation()/getTranslation(), although I'm not sure if this solution is technically ideal (since it overrides the odometry's rotation/translation component).

  public void resetTranslation(Translation2d translation) {
    resetPose(new Pose2d(translation, m_poseEstimate.getRotation());
  }

  public void resetRotation(Rotation2d rotation) {
    resetPose(new Pose2d(m_poseEstimate.getTranslation(), rotation);
  }

The more proper solution is to use the latest vision update to compensate the rotation/translation of the odometry pose.