From f4db88da9a3335f5fedb91ccfd4dcde7e9b7ace4 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 1 Nov 2025 14:45:53 -0700 Subject: [PATCH 1/5] [build] Document how to mirror new Doxygen versions (#8327) --- docs/build.gradle | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/build.gradle b/docs/build.gradle index 7071311dff..5a3784a883 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -54,6 +54,10 @@ doxygen { // https://frcmaven.wpi.edu/ui/native/generic-release-mirror/doxygen/, which // is a mirror of binaries from https://doxygen.nl/download.html. // + // To mirror a new Doxygen version, retrigger the GitHub Actions workflow in + // https://github.com/wpilibsuite/doxygen-mirror with the desired version + // number as an input. + // // Ensure theme.css (from https://github.com/jothepro/doxygen-awesome-css) // is compatible with Doxygen version when updating. executables { From e207ca48801e46b5432b36c074bffb541bf8b540 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 7 Nov 2025 10:07:56 -0800 Subject: [PATCH 2/5] [build] Clean up spotbugs excludes (#8332) --- styleguide/spotbugs-exclude.xml | 223 +++++++++++++++----------------- 1 file changed, 101 insertions(+), 122 deletions(-) diff --git a/styleguide/spotbugs-exclude.xml b/styleguide/spotbugs-exclude.xml index f3fbffb591..4102891c3d 100644 --- a/styleguide/spotbugs-exclude.xml +++ b/styleguide/spotbugs-exclude.xml @@ -2,45 +2,32 @@ - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + - + + + + + @@ -49,16 +36,32 @@ + + + + + + + + + + + + - - - - + + + + + @@ -77,15 +80,24 @@ + + + + + + + + + + + + + - - - - @@ -94,46 +106,33 @@ - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - + + + + @@ -141,78 +140,49 @@ + - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - - + - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - + + + + @@ -223,5 +193,14 @@ + + + + + + + + + From 02252b58d7bdf91d2c4c4ac38788c84087cba60b Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 8 Nov 2025 02:06:39 +0000 Subject: [PATCH 3/5] [build] Update to 2026 Beta 1 (#8337) --- shared/config.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/config.gradle b/shared/config.gradle index 4776cfdecf..69707713ef 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -14,7 +14,7 @@ nativeUtils { wpi { configureDependencies { opencvYear = "frc2025" - niLibVersion = "2025.2.0" + niLibVersion = "2026.1.0" opencvVersion = "4.10.0-3" } } From 688535298b8ed0283ba9adf0de55fae6e6379901 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 7 Nov 2025 18:07:05 -0800 Subject: [PATCH 4/5] [cscore] Add braces to match styleguide (NFC) (#8339) --- cscore/src/main/native/windows/UsbCameraImpl.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/cscore/src/main/native/windows/UsbCameraImpl.cpp b/cscore/src/main/native/windows/UsbCameraImpl.cpp index 516e33baaa..684905f997 100644 --- a/cscore/src/main/native/windows/UsbCameraImpl.cpp +++ b/cscore/src/main/native/windows/UsbCameraImpl.cpp @@ -255,8 +255,9 @@ bool UsbCameraImpl::CheckDeviceChange(WPARAM wParam, DEV_BROADCAST_HDR* pHdr, } void UsbCameraImpl::DeviceDisconnect() { - if (m_connectVerbose) + if (m_connectVerbose) { SINFO("Disconnected from {}", m_path); + } m_sourceReader.Reset(); m_mediaSource.Reset(); if (m_imageCallback) { @@ -655,8 +656,9 @@ void UsbCameraImpl::DeviceCacheProperty( rawProp->valueStr = perProp->valueStr; // copy } else { // Read current raw value and set percentage from it - if (!rawProp->DeviceGet(lock, pProcAmp)) + if (!rawProp->DeviceGet(lock, pProcAmp)) { SWARNING("failed to get property {}", rawProp->name); + } if (perProp) { perProp->SetValue(RawToPercentage(*rawProp, rawProp->value)); @@ -666,8 +668,9 @@ void UsbCameraImpl::DeviceCacheProperty( // Set value on device if user-configured if (rawProp->valueSet) { - if (!rawProp->DeviceSet(lock, pProcAmp)) + if (!rawProp->DeviceSet(lock, pProcAmp)) { SWARNING("failed to set property {}", rawProp->name); + } } // Update pointers since we released the lock From b8d6bc2eb1b6cea10d1179939114d041945e172a Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 7 Nov 2025 18:07:43 -0800 Subject: [PATCH 5/5] [wpimath] Scale transforms instead of twists in PoseEstimator (#8333) The spiraling issue occurs when the vision rotation standard deviation is very high relative to the odometry rotation standard deviation and the vision measurements have a large rotation error. (Scaling the rotation component of a twist without scaling the translation component causes the direction of overall translation to change, leading to spiraling around (either towards or away) the vision measurement instead of moving towards it.) Using a transform instead of a twist avoids this issue. In general, scaling twist components is more mathematically correct than scaling transform components. However, although twists are correct for modeling uncertainty in an odometry-only pose estimate, they are not correct for the difference between the odometry-only pose estimate and a vision measurement. Since neither twists nor transforms are completely correct (and the pose estimator as a whole is not mathematically correct), but using transforms can guarantee that the pose estimate approaches the vision measurement (instead of potentially spiraling away), they are the least bad option. --- .../first/math/estimator/PoseEstimator.java | 26 +++++++---- .../first/math/estimator/PoseEstimator3d.java | 41 ++++++++++------- .../include/frc/estimator/PoseEstimator.h | 31 +++++++------ .../include/frc/estimator/PoseEstimator3d.h | 45 +++++++++++-------- 4 files changed, 85 insertions(+), 58 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 1f00cb7b18..4e9bf4b3e8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -11,8 +11,8 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; @@ -273,19 +273,27 @@ public class PoseEstimator { return; } - // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + // Step 4: Measure the transform between the old pose estimate and the vision pose. + var transform = visionRobotPoseMeters.minus(visionSample.get()); - // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 5: We should not trust the transform entirely, so instead we scale this transform by a + // Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta)); + var k_times_transform = + m_visionK.times( + VecBuilder.fill( + transform.getX(), transform.getY(), transform.getRotation().getRadians())); - // Step 6: Convert back to Twist2d. - var scaledTwist = - new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); + // Step 6: Convert back to Transform2d. + var scaledTransform = + new Transform2d( + k_times_transform.get(0, 0), + k_times_transform.get(1, 0), + Rotation2d.fromRadians(k_times_transform.get(2, 0))); // Step 7: Calculate and record the vision update. - var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + var visionUpdate = + new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestampSeconds, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 9ce96f0243..463cb9481c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -13,9 +13,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry3d; @@ -286,27 +286,36 @@ public class PoseEstimator3d { return; } - // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + // Step 4: Measure the transform between the old pose estimate and the vision pose. + var transform = visionRobotPoseMeters.minus(visionSample.get()); - // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // Step 5: We should not trust the transform entirely, so instead we scale this transform by a + // Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. - var k_times_twist = + var k_times_transform = m_visionK.times( - VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz)); + VecBuilder.fill( + transform.getX(), + transform.getY(), + transform.getZ(), + transform.getRotation().getX(), + transform.getRotation().getY(), + transform.getRotation().getZ())); - // Step 6: Convert back to Twist3d. - var scaledTwist = - new Twist3d( - k_times_twist.get(0, 0), - k_times_twist.get(1, 0), - k_times_twist.get(2, 0), - k_times_twist.get(3, 0), - k_times_twist.get(4, 0), - k_times_twist.get(5, 0)); + // Step 6: Convert back to Transform3d. + var scaledTransform = + new Transform3d( + k_times_transform.get(0, 0), + k_times_transform.get(1, 0), + k_times_transform.get(2, 0), + new Rotation3d( + k_times_transform.get(3, 0), + k_times_transform.get(4, 0), + k_times_transform.get(5, 0))); // Step 7: Calculate and record the vision update. - var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + var visionUpdate = + new VisionUpdate(visionSample.get().plus(scaledTransform), odometrySample.get()); m_visionUpdates.put(timestampSeconds, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 9c3bc4d6de..1aaf4a39d8 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -14,6 +14,7 @@ #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" #include "frc/geometry/Translation2d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" @@ -261,24 +262,26 @@ class WPILIB_DLLEXPORT PoseEstimator { return; } - // Step 4: Measure the twist between the old pose estimate and the vision - // pose. - auto twist = visionSample.value().Log(visionRobotPose); + // Step 4: Measure the transform between the old pose estimate and the + // vision transform. + auto transform = visionRobotPose - visionSample.value(); - // 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 5: We should not trust the transform entirely, so instead we scale + // this transform by a Kalman gain matrix representing how much we trust + // vision measurements compared to our current pose. + Eigen::Vector3d k_times_transform = + m_visionK * Eigen::Vector3d{transform.X().value(), + transform.Y().value(), + transform.Rotation().Radians().value()}; - // 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 6: Convert back to Transform2d. + Transform2d scaledTransform{ + units::meter_t{k_times_transform(0)}, + units::meter_t{k_times_transform(1)}, + Rotation2d{units::radian_t{k_times_transform(2)}}}; // Step 7: Calculate and record the vision update. - VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample}; m_visionUpdates[timestamp] = visionUpdate; // Step 8: Remove later vision measurements. (Matches previous behavior) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index bc8bdb08a6..ecf9ccbf88 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -14,9 +14,10 @@ #include #include "frc/EigenCore.h" -#include "frc/geometry/Pose2d.h" -#include "frc/geometry/Rotation2d.h" -#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/geometry/Transform3d.h" +#include "frc/geometry/Translation3d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" #include "frc/kinematics/Odometry3d.h" @@ -270,26 +271,32 @@ class WPILIB_DLLEXPORT PoseEstimator3d { return; } - // Step 4: Measure the twist between the old pose estimate and the vision - // pose. - auto twist = visionSample.value().Log(visionRobotPose); + // Step 4: Measure the transform between the old pose estimate and the + // vision pose. + auto transform = visionRobotPose - visionSample.value(); - // 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. - frc::Vectord<6> k_times_twist = - m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(), - twist.dz.value(), twist.rx.value(), - twist.ry.value(), twist.rz.value()}; + // Step 5: We should not trust the transform entirely, so instead we scale + // this transform by a Kalman gain matrix representing how much we trust + // vision measurements compared to our current pose. + frc::Vectord<6> k_times_transform = + m_visionK * frc::Vectord<6>{transform.X().value(), + transform.Y().value(), + transform.Z().value(), + transform.Rotation().X().value(), + transform.Rotation().Y().value(), + transform.Rotation().Z().value()}; - // Step 6: Convert back to Twist3d. - Twist3d scaledTwist{ - units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)}, - units::meter_t{k_times_twist(2)}, units::radian_t{k_times_twist(3)}, - units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}}; + // Step 6: Convert back to Transform3d. + Transform3d scaledTransform{ + units::meter_t{k_times_transform(0)}, + units::meter_t{k_times_transform(1)}, + units::meter_t{k_times_transform(2)}, + Rotation3d{units::radian_t{k_times_transform(3)}, + units::radian_t{k_times_transform(4)}, + units::radian_t{k_times_transform(5)}}}; // Step 7: Calculate and record the vision update. - VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + VisionUpdate visionUpdate{*visionSample + scaledTransform, *odometrySample}; m_visionUpdates[timestamp] = visionUpdate; // Step 8: Remove later vision measurements. (Matches previous behavior)