[wpimath] Simplify pose estimator

[KangarooKoala]

Completely non-breaking pose estimator implementation parts of #5473 (that is, excluding the removal of the kinematics parameter of PoseEstimator (which is exposed to custom implementations) and the test unification) updated for the addition of sampleAt(). (Thanks @jlmcmchl for the inspiration!)

Allocations analysis:

• This assumes 0 allocations for map insertion, isEmpty(), firstKey()/lastKey(), floorKey(), and entrySet() and 1 allocation for headMap() and tailMap().

• sampleAt() (Net -2 for diffy, -4 for mecanum, and -5 - 5n for swerve):

  • Savings: An InterpolationRecord sample (if interpolating, 1 wheel positions interpolation, 6 allocations, 1 kinematics.toTwist2d() call).
  • Additions: An odometry pose sample (if interpolating, 4 allocations) and a vision compensation (2 allocations).

• updateWithTime() (Net 0 for diffy, 0 for mecanum, and -n for swerve):

  • Savings: Copying the wheel positions (1 wheel positions copy) and allocating an InterpolationRecord object (1 allocation).
  • Additions: Updating latest position (2 allocations).

• addVisionMeasurement() (With k odometry updates after vision measurement: Net +7 - 8k for diffy, +5 - 10k for mecanum, and +4 - 5n - (11 + 3n)k for swerve):

  • Savings: An InterpolationRecord sample (if interpolating, 1 wheel positions interpolation, 6 allocations, 1 kinematics.toTwist2d() call) and an updateWithTime() call for every single odometry update after the vision measurement (1 odometry update (4 allocations, 1 kinematics.toTwist2d() call, 1 wheel positions copy), 1 wheel positions copy, 1 allocation).
  • Additions: Vision update cleanup (1 allocation), an odometry pose sample (if interpolating, 4 allocations), a sampleAt() call (6 allocations), a VisionUpdate allocation (1 allocation), clearing latest vision updates (1 allocation), and updating latest pose estimate (2 allocations).

  Costs of kinematics operations:

  	Differential	Mecanum	Swerve (n modules)
  Wheel positions copy	1	1	1 + n
  Wheel positions interpolation	1	1	1 + 4n
  kinematics.toTwist2d()	1	3	4 + n

Notes:

• Because of the need to support sampleAt(), we must maintain a record of past vision updates. This is the primary reason why this PR has a much greater line count than the original PR (which was able to effectively only have 1 offset). Changing the behavior of sampleAt() would allow for the complete removal of m_visionUpdates, but the goal of this PR was to be a completely non-visible change.
• m_visionUpdates cannot be a TimeInterpolatableBuffer, because a vision update may be needed for an indefinite amount of time (if no more vision updates come after).
• Step 8 of adding a vision measurement (clearing updates after the added one) is only to maintain previous behavior- Because previously vision measurements were just interpolation records (with poses besides what would expected from the odometry update information) and previously all interpolation records were replayed as odometry updates, the pose information was lost. My intent was for this PR to have no changes to existing behavior and then change the behavior to update the poses of later updates in a separate PR, but if people want that change done in this PR, I can do that too.
• Right now, the PoseEstimator constructor keeps around the (now unused) kinematics parameter in case people implemented a custom implementation of PoseEstimator. We could just remove it now, though, since the expected number of people in that category is quite low (and those people would probably already have to make updates because of [wpimath] Remove swerve wrappers for odometry and pose estimation, move wheel positions operations to kinematics #6673).
• The only API change (that I know of) is adding a const version of GetInternalBuffer() to C++'s TimeInterpolatableBuffer. This could be moved to a separate PR for commit history if desired, but I included it in this PR to make the reason obvious.

[calcmogul]

Since sampleAt() was merged after 2024 champs, it hasn't been in a release yet, so we can change the behavior to whatever we want.

[KangarooKoala]

I didn't think of this earlier, but the information is still needed for addVisionMeasurement() because we sample what the estimate of what the robot pose was at the time of the new vision measurement, but using a single offset would suffice for the main case of vision measurements coming with increasing timestamps. (And from previous discussion in Discord, that's a rare case we could probably drop support for. There's already weird and somewhat unexpected behavior when that happens.)

[virtuald]

Sounds like you should write some real code that uses this to make sure it actually does what a user might want before finalizing the PR.

[KangarooKoala]

I didn't think of this earlier, but the information is still needed for addVisionMeasurement() because we sample what the estimate of what the robot pose was at the time of the new vision measurement, but using a single offset would suffice for the main case of vision measurements coming with increasing timestamps. (And from previous discussion in Discord, that's a rare case we could probably drop support for. There's already weird and somewhat unexpected behavior when that happens.)

Based on further conversation on Discord, we don't want to add the assumption that vision measurements come in order (which is something that would happen with multiple coprocessors), so we'll keep the vision update buffer.

Sounds like you should write some real code that uses this to make sure it actually does what a user might want before finalizing the PR.

Is there something in addition to the existing tests that you think should be added? (Maybe a simulation of multiple coprocessors?)

[KangarooKoala]

What's left for this PR? I'd be happy to add some tests (probably in a separate PR), but I'd need to know which behavior we want to test.

[calcmogul]

Please use blank lines to delimit related chunks of code. I'm having trouble reading and understanding it.

[KangarooKoala]

From local testing, the difference between the old implementation and the new implementation is on the order of 1e-12. (First, I changed debug to true in calls to testFollowTrajectory in SwerveDrivePoseEstimatorTest.cpp, then piped stdout from ./gradlew wpimath:testDesktopCpp into a text file, did the same on main (e2893fc), and then ran a Python script on the two text files to find the greatest difference.)

Python code:

from decimal import Decimal, InvalidOperation
import sys

with open(sys.argv[1]) as reader1, open(sys.argv[2]) as reader2:
  max_err: Decimal = Decimal(-1)
  for (lnum, (line1, line2)) in enumerate(zip(reader1, reader2, strict=True), start=1):
    col1: int = 0
    col2: int = 0
    while True:
      # -1 if not found, which using as the end index in a substring cuts off the last one (newline character)
      next1: int = line1.find(',', col1)
      next2: int = line2.find(',', col2)
      part1: str = line1[col1:next1].strip()
      part2: str = line2[col2:next2].strip()
      float1: None | Decimal
      try:
        float1 = Decimal(part1)
      except InvalidOperation:
        float1 = None
      float2: None | Decimal
      try:
        float2 = Decimal(part2)
      except InvalidOperation:
        float2 = None
      if (float1 is None) ^ (float2 is None):
        print(f"mismatched type: {part1 = !r}, {part2 = !r}")
      elif float1 is not None and float2 is not None:
        err: Decimal = abs(float2 - float1)
        if err > max_err:
          max_err = err
          print(f"New max error {err} at {lnum} between {part1} and {part2}")
      if next1 < 0 or next2 < 0:
        break
      col1 = next1 + 1
      col2 = next2 + 1

Output:

New max error 0 at 47 between 0 and 0
New max error 1E-17 at 60 between 0.04050561270097854 and 0.04050561270097855
New max error 3E-16 at 60 between 0.7451136041863254 and 0.7451136041863257
New max error 1.77E-15 at 77 between 0.2627713934009313 and 0.26277139340092953
New max error 1.8E-15 at 81 between 0.3399786876534412 and 0.3399786876534394
New max error 1.20E-14 at 108 between 1.1662427755618154 and 1.1662427755618034
New max error 1.22E-14 at 110 between 1.2420949917467707 and 1.2420949917467585
New max error 5.07E-14 at 590 between 0.7201715575416567 and 0.7201715575417074
New max error 5.79E-14 at 590 between 0.7198043229924798 and 0.7198043229925377
New max error 5.80E-14 at 591 between 0.7361595510115062 and 0.7361595510115642
New max error 1.2005E-13 at 765 between 0.41930745110901085 and 0.4193074511091309
New max error 1.202E-13 at 765 between -0.3931206122099427 and -0.3931206122100629
New max error 7.313E-13 at 3434 between -2.991012647154576 and -2.9910126471538447
New max error 7.314E-13 at 3436 between -2.973068699945149 and -2.9730686999444176
New max error 4.0153E-12 at 4402 between 0.9545146075830452 and 0.9545146075870605
New max error 4.0154E-12 at 4403 between 0.9634643004179145 and 0.9634643004219299
New max error 4.0202E-12 at 8490 between 0.9545729565420277 and 0.9545729565460479
New max error 4.0204E-12 at 8491 between 0.963522649376897 and 0.9635226493809174
New max error 4.0205E-12 at 8494 between 0.9808667964291787 and 0.9808667964331992