Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Cache pose calculations in PhotonPoseEstimator (#788)

Browse Source 
* Add pose caching to Java

* Refactor strategy fallthrough

* Hopefully add pose caching to C++

* Make Java switch same order as enum and C++ switch

* C++ absolute value in timestamp check

* Fix Java NPE

* Use `units::second_t` in timestamp

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

* Expand Java unit test

* Copy comments into C++

* Add tests to C++

* Run format

* Update PhotonCamera.cpp

* Probably fix bad access exception

* a

* init timestamp

* Remove prints

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Joseph Eng <joseng2358@gmail.com>
This commit is contained in:
Joseph Eng
2023-02-13 18:22:22 -08:00
committed by GitHub
parent 5b86360b9b
commit 545e016d04
6 changed files with 198 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -78,6 +78,7 @@ public class PhotonPoseEstimator {
private Pose3d lastPose;
private Pose3d referencePose;
protected double poseCacheTimestampSeconds = -1;
private final Set<Integer> reportedErrors = new HashSet<>();
/**
@@ -105,6 +106,17 @@ public class PhotonPoseEstimator {
this.robotToCamera = robotToCamera;
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
}
private void checkUpdate(Object oldObj, Object newObj) {
if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
invalidatePoseCache();
}
}
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
@@ -120,6 +132,7 @@ public class PhotonPoseEstimator {
* @param fieldTags the AprilTagFieldLayout
*/
public void setFieldTags(AprilTagFieldLayout fieldTags) {
checkUpdate(this.fieldTags, fieldTags);
this.fieldTags = fieldTags;
}
@@ -138,6 +151,7 @@ public class PhotonPoseEstimator {
* @param strategy the strategy to set
*/
public void setPrimaryStrategy(PoseStrategy strategy) {
checkUpdate(this.primaryStrategy, strategy);
this.primaryStrategy = strategy;
}
@@ -148,6 +162,7 @@ public class PhotonPoseEstimator {
* @param strategy the strategy to set
*/
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null);
@@ -172,6 +187,7 @@ public class PhotonPoseEstimator {
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose3d referencePose) {
checkUpdate(this.referencePose, referencePose);
this.referencePose = referencePose;
}
@@ -182,7 +198,7 @@ public class PhotonPoseEstimator {
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
this.referencePose = new Pose3d(referencePose);
setReferencePose(new Pose3d(referencePose));
}
/**
@@ -247,6 +263,22 @@ public class PhotonPoseEstimator {
* pipeline results used to create the estimate
*/
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return Optional.empty();
}
// If the pose cache timestamp was set, and the result is from the same timestamp, return an
// empty result
if (poseCacheTimestampSeconds > 0
&& Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) {
return Optional.empty();
}
// Remember the timestamp of the current result used
poseCacheTimestampSeconds = cameraResult.getTimestampSeconds();
// If no targets seen, trivial case -- return empty result
if (!cameraResult.hasTargets()) {
return Optional.empty();
}
@@ -264,9 +296,11 @@ public class PhotonPoseEstimator {
case CLOSEST_TO_CAMERA_HEIGHT:
estimatedPose = closestToCameraHeightStrategy(cameraResult);
break;
case CLOSEST_TO_REFERENCE_POSE:
estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
break;
case CLOSEST_TO_LAST_POSE:
setReferencePose(lastPose);
case CLOSEST_TO_REFERENCE_POSE:
estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
break;
case AVERAGE_BEST_TARGETS:

5
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
View File

@@ -74,7 +74,10 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
: PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
PhotonPipelineResult PhotonCamera::GetLatestResult() {
if (test) return testResult;
if (test) {
return testResult;
}
// Prints warning if not connected
VerifyVersion();

30
photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
View File

@@ -24,6 +24,7 @@
#include "photonlib/PhotonPoseEstimator.h"
#include <cmath>
#include <iostream>
#include <limits>
#include <map>
@@ -39,6 +40,7 @@
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <units/math.h>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
@@ -64,7 +66,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
camera(std::move(cam)),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP) {
@@ -74,6 +77,9 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
"");
strategy = LOWEST_AMBIGUITY;
}
if (this->multiTagFallbackStrategy != strategy) {
InvalidatePoseCache();
}
multiTagFallbackStrategy = strategy;
}
@@ -84,6 +90,22 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
return std::nullopt;
}
// If the pose cache timestamp was set, and the result is from the same
// timestamp, return an empty result
if (poseCacheTimestamp > 0_s &&
units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < 0.001_ms) {
return std::nullopt;
}
// Remember the timestamp of the current result used
poseCacheTimestamp = result.GetTimestamp();
// If no targets seen, trivial case -- return empty result
if (!result.HasTargets()) {
return std::nullopt;
}
@@ -118,11 +140,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
return std::nullopt;
}
if (!ret) {
// TODO
ret = std::nullopt;
}
return ret;

14
photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
View File

@@ -109,7 +109,12 @@ class PhotonPoseEstimator {
*
* @param strategy the strategy to set
*/
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
inline void SetPoseStrategy(PoseStrategy strat) {
if (strategy != strat) {
InvalidatePoseCache();
}
strategy = strat;
}
/**
* Set the Position Estimation Strategy used in multi-tag mode when
@@ -133,6 +138,9 @@ class PhotonPoseEstimator {
* @param referencePose the referencePose to set
*/
inline void SetReferencePose(frc::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
}
this->referencePose = referencePose;
}
@@ -186,6 +194,10 @@ class PhotonPoseEstimator {
frc::Pose3d lastPose;
frc::Pose3d referencePose;
units::second_t poseCacheTimestamp;
inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
PoseStrategy strategy);

68
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -25,6 +25,8 @@
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
@@ -384,6 +386,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setTimestampSeconds(1);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -469,6 +472,71 @@ class PhotonPoseEstimatorTest {
assertEquals(1, pose.getZ(), .01);
}
@Test
void cacheIsInvalidated() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
var result =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
result.setTimestampSeconds(20);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.setTimestampSeconds(1);
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update();
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update();
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Set new field layout -- right after, the pose cache timestamp should be -1
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update();
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
}
@Test
void averageBestPoses() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();

55
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -231,6 +231,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
@@ -257,12 +258,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};
estimator.GetCamera().testResult = {2_ms, targetsThree};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(7));
estimator.GetCamera().testResult.SetTimestamp(units::second_t(21));
estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.value().timestamp),
EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
@@ -310,3 +312,52 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, PoseCache) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
// empty input, expect empty out
estimator.GetCamera().testResult = {2_ms, {}};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(1));
auto estimatedPose = estimator.Update();
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
estimator.GetCamera().testResult = {3_ms, targets};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(15));
estimatedPose = estimator.Update();
EXPECT_TRUE(estimatedPose);
EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- now pose cache should be empty
estimatedPose = estimator.Update();
EXPECT_FALSE(estimatedPose);
}