Add C++

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -21,5 +21,5 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     const wpi::array<double, 3>& visionMeasurementStdDevs)
     : PoseEstimator<DifferentialDriveWheelSpeeds,
                     DifferentialDriveWheelPositions>(
-          kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+          m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
       m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -25,5 +25,5 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
     : PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
-          kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+          m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
       m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}

wpimath/src/main/native/include/frc/estimator/PoseEstimator.h

@@ -4,14 +4,16 @@
 
 #pragma once
 
+#include <optional>
+
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Twist2d.h"
 #include "frc/interpolation/TimeInterpolatableBuffer.h"
-#include "frc/kinematics/Kinematics.h"
 #include "frc/kinematics/Odometry.h"
 #include "frc/kinematics/WheelPositions.h"
 #include "units/time.h"
@@ -37,8 +39,6 @@ class WPILIB_DLLEXPORT PoseEstimator {
   /**
    * Constructs a PoseEstimator.
    *
-   * @param kinematics A correctly-configured kinematics object for your
-   *     drivetrain.
    * @param odometry A correctly-configured odometry object for your drivetrain.
    * @param stateStdDevs Standard deviations of the pose estimate (x position in
    *     meters, y position in meters, and heading in radians). Increase these
@@ -48,8 +48,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
    *     radians). Increase these numbers to trust the vision pose measurement
    *     less.
    */
-  PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
-                Odometry<WheelSpeeds, WheelPositions>& odometry,
+  PoseEstimator(Odometry<WheelSpeeds, WheelPositions>& odometry,
                 const wpi::array<double, 3>& stateStdDevs,
                 const wpi::array<double, 3>& visionMeasurementStdDevs);
 
@@ -86,6 +85,14 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   Pose2d GetEstimatedPosition() const;
 
+  /**
+   * Return the pose at a given timestamp, if one exists.
+   *
+   * @param timestamp The pose's timestamp.
+   * @return The pose at a given timestamp, if one exists.
+   */
+  std::optional<Pose2d> SampleAt(units::second_t timestamp) const;
+
   /**
    * Adds a vision measurement to the Kalman Filter. This will correct
    * the odometry pose estimate while still accounting for measurement noise.
@@ -171,57 +178,25 @@ class WPILIB_DLLEXPORT PoseEstimator {
                         const WheelPositions& wheelPositions);
 
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyroscope angle.
-    Rotation2d gyroAngle;
-
-    // The distances traveled by the wheels.
-    WheelPositions wheelPositions;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(
-        Kinematics<WheelSpeeds, WheelPositions>& kinematics,
-        InterpolationRecord endValue, double i) const;
-  };
-
   static constexpr units::second_t kBufferDuration = 1.5_s;
 
-  Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
   Odometry<WheelSpeeds, WheelPositions>& m_odometry;
   wpi::array<double, 3> m_q{wpi::empty_array};
   Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
 
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      kBufferDuration, [this](const InterpolationRecord& start,
-                              const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
+  TimeInterpolatableBuffer<Pose2d> m_poseBuffer{
+      kBufferDuration, [](const Pose2d& start, const Pose2d& end, double t) {
+        if (t <= 0) {
+          return start;
+        } else if (t >= 1) {
+          return end;
+        } else {
+          auto twist = start.Log(end);
+          Twist2d scaledTwist{twist.dx * t, twist.dy * t, twist.dtheta * t};
+          return start.Exp(scaledTwist);
+        }
       }};
+  Pose2d m_poseEstimate;
 };
 }  // namespace frc
 

wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc

@@ -10,11 +10,10 @@ namespace frc {
 
 template <typename WheelSpeeds, WheelPositions WheelPositions>
 PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
-    Kinematics<WheelSpeeds, WheelPositions>& kinematics,
     Odometry<WheelSpeeds, WheelPositions>& odometry,
     const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
-    : m_kinematics(kinematics), m_odometry(odometry) {
+    : m_odometry(odometry), m_poseEstimate{odometry.GetPose()} {
   for (size_t i = 0; i < 3; ++i) {
     m_q[i] = stateStdDevs[i] * stateStdDevs[i];
   }
@@ -49,12 +48,25 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
   // Reset state estimate and error covariance
   m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
   m_poseBuffer.Clear();
+  m_poseEstimate = m_odometry.GetPose();
 }
 
 template <typename WheelSpeeds, WheelPositions WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
     const {
-  return m_odometry.GetPose();
+  return m_poseEstimate;
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
+    units::second_t timestamp) const {
+  // TODO Use C++23 std::optional::transform
+  std::optional<frc::Pose2d> sample = m_poseBuffer.Sample(timestamp);
+  if (sample) {
+    return *sample + Transform2d{m_odometry.GetPose(), m_poseEstimate};
+  } else {
+    return std::nullopt;
+  }
 }
 
 template <typename WheelSpeeds, WheelPositions WheelPositions>
@@ -75,44 +87,33 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
     return;
   }
 
-  // Step 2: Measure the twist between the odometry pose and the vision pose
-  auto twist = sample.value().pose.Log(visionRobotPose);
+  // Step 2: Record the odometry updates that have occurred since the vision
+  // measurement
+  Transform2d odometry_backtrack{m_odometry.GetPose(), *sample};
+  Transform2d odometry_forward{*sample, m_odometry.GetPose()};
+
+  // Step 3: Revert said odometry updates to get a state estimate from when the
+  // vision measurement occurred.
+  auto old_estimate = m_poseEstimate + odometry_backtrack;
 
-  // Step 3: We should not trust the twist entirely, so instead we scale this
+  // Step 4: Measure the twist between the odometry pose and the vision pose.
+  auto twist = old_estimate.Log(visionRobotPose);
+
+  // Step 5: We should not trust the twist entirely, so instead we scale this
   // twist by a Kalman gain matrix representing how much we trust vision
   // measurements compared to our current pose.
   Eigen::Vector3d k_times_twist =
       m_visionK *
       Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
 
-  // Step 4: Convert back to Twist2d
+  // Step 6: Convert back to Twist2d.
   Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
                       units::meter_t{k_times_twist(1)},
                       units::radian_t{k_times_twist(2)}};
 
-  // Step 5: Reset Odometry to state at sample with vision adjustment.
-  m_odometry.ResetPosition(sample.value().gyroAngle,
-                           sample.value().wheelPositions,
-                           sample.value().pose.Exp(scaledTwist));
-
-  // Step 6: Record the current pose to allow multiple measurements from the
-  // same timestamp
-  m_poseBuffer.AddSample(timestamp,
-                         {GetEstimatedPosition(), sample.value().gyroAngle,
-                          sample.value().wheelPositions});
-
-  // Step 7: Replay odometry inputs between sample time and latest recorded
-  // sample to update the pose buffer and correct odometry.
-  auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-  auto upper_bound =
-      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
-                       [](const auto& pair, auto t) { return t > pair.first; });
-
-  for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
-    UpdateWithTime(entry->first, entry->second.gyroAngle,
-                   entry->second.wheelPositions);
-  }
+  // Step 7: Apply this adjustment to the old estimate, then replay dometry
+  // updates.
+  m_poseEstimate = old_estimate.Exp(scaledTwist) + odometry_forward;
 }
 
 template <typename WheelSpeeds, WheelPositions WheelPositions>
@@ -126,40 +127,13 @@ template <typename WheelSpeeds, WheelPositions WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
     units::second_t currentTime, const Rotation2d& gyroAngle,
     const WheelPositions& wheelPositions) {
-  m_odometry.Update(gyroAngle, wheelPositions);
-
-  WheelPositions internalWheelPositions = wheelPositions;
+  auto lastOdom = m_odometry.GetPose();
+  auto currOdom = m_odometry.Update(gyroAngle, wheelPositions);
+  m_poseBuffer.AddSample(currentTime, currOdom);
 
-  m_poseBuffer.AddSample(
-      currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
+  m_poseEstimate = m_poseEstimate.Exp(lastOdom.Log(currOdom));
 
   return GetEstimatedPosition();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
-typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
-PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
-    Kinematics<WheelSpeeds, WheelPositions>& kinematics,
-    InterpolationRecord endValue, double i) const {
-  if (i < 0) {
-    return *this;
-  } else if (i > 1) {
-    return endValue;
-  } else {
-    // Find the new wheel distance measurements.
-    WheelPositions wheels_lerp =
-        this->wheelPositions.Interpolate(endValue.wheelPositions, i);
-
-    // Find the new gyro angle.
-    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-    // Create a twist to represent the change based on the interpolated
-    // sensor inputs.
-    auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
-    twist.dtheta = (gyro - gyroAngle).Radians();
-
-    return {pose.Exp(twist), gyro, wheels_lerp};
-  }
-}
-
 }  // namespace frc

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -85,7 +85,7 @@ class SwerveDrivePoseEstimator
       const wpi::array<double, 3>& visionMeasurementStdDevs)
       : PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
                       SwerveDriveWheelPositions<NumModules>>(
-            kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+            m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
         m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
 
   /**

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -100,7 +100,7 @@ class TimeInterpolatableBuffer {
    *
    * @param time The time at which to sample the buffer.
    */
-  std::optional<T> Sample(units::second_t time) {
+  std::optional<T> Sample(units::second_t time) const {
     if (m_pastSnapshots.empty()) {
       return {};
     }

wpimath/src/test/native/cpp/SamplingUtil.h

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <random>
+
+#include <wpi/array.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Twist2d.h"
+
+namespace frc {
+
+template <std::uniform_random_bit_generator Generator>
+frc::Rotation2d SampleRotation2d(Generator& generator,
+                                 std::normal_distribution<double>& distribution,
+                                 double stdDev) {
+  return frc::Rotation2d{units::radian_t{distribution(generator) * stdDev}};
+}
+
+template <std::uniform_random_bit_generator Generator>
+frc::Twist2d SampleTwist2d(Generator& generator,
+                           std::normal_distribution<double>& distribution,
+                           wpi::array<double, 3> stdDev) {
+  return frc::Twist2d{units::meter_t{distribution(generator) * stdDev[0]},
+                      units::meter_t{distribution(generator) * stdDev[1]},
+                      units::radian_t{distribution(generator) * stdDev[2]}};
+}
+
+}  // namespace frc