Skip to content

Commit

Permalink
Cache pose calculations in PhotonPoseEstimator (PhotonVision#788)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>

* 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 <[email protected]>
Co-authored-by: Joseph Eng <[email protected]>
  • Loading branch information
@KangarooKoala @mcm001
3 people authored Feb 14, 2023
1 parent 5b86360 commit 545e016
Show file tree
Hide file tree
Showing 6 changed files with 198 additions and 12 deletions.
38 changes: 36 additions & 2 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ public enum PoseStrategy {

private Pose3d lastPose;
private Pose3d referencePose;
protected double poseCacheTimestampSeconds = -1;
private final Set<Integer> reportedErrors = new HashSet<>();

/**
Expand Down Expand Up @@ -105,6 +106,17 @@ public 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.
*
Expand All @@ -120,6 +132,7 @@ public AprilTagFieldLayout getFieldTags() {
* @param fieldTags the AprilTagFieldLayout
*/
public void setFieldTags(AprilTagFieldLayout fieldTags) {
checkUpdate(this.fieldTags, fieldTags);
this.fieldTags = fieldTags;
}

Expand All @@ -138,6 +151,7 @@ public PoseStrategy getPrimaryStrategy() {
* @param strategy the strategy to set
*/
public void setPrimaryStrategy(PoseStrategy strategy) {
checkUpdate(this.primaryStrategy, strategy);
this.primaryStrategy = strategy;
}

Expand All @@ -148,6 +162,7 @@ public void setPrimaryStrategy(PoseStrategy strategy) {
* @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);
Expand All @@ -172,6 +187,7 @@ public Pose3d getReferencePose() {
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose3d referencePose) {
checkUpdate(this.referencePose, referencePose);
this.referencePose = referencePose;
}

Expand All @@ -182,7 +198,7 @@ public void setReferencePose(Pose3d referencePose) {
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
this.referencePose = new Pose3d(referencePose);
setReferencePose(new Pose3d(referencePose));
}

/**
Expand Down Expand Up @@ -247,6 +263,22 @@ public Optional<EstimatedRobotPose> update() {
* 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();
}
Expand All @@ -264,9 +296,11 @@ private Optional<EstimatedRobotPose> update(
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:
Expand Down
5 changes: 4 additions & 1 deletion photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down
30 changes: 24 additions & 6 deletions photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include "photonlib/PhotonPoseEstimator.h"

#include <cmath>
#include <iostream>
#include <limits>
#include <map>
Expand All @@ -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"
Expand All @@ -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) {
Expand All @@ -74,6 +77,9 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
"");
strategy = LOWEST_AMBIGUITY;
}
if (this->multiTagFallbackStrategy != strategy) {
InvalidatePoseCache();
}
multiTagFallbackStrategy = strategy;
}

Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down
14 changes: 13 additions & 1 deletion photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
}

Expand Down Expand Up @@ -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);

Expand Down
68 changes: 68 additions & 0 deletions photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -384,6 +386,7 @@ void closestToLastPose() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setTimestampSeconds(1);

PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
Expand Down Expand Up @@ -469,6 +472,71 @@ void closestToLastPose() {
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();
Expand Down
55 changes: 53 additions & 2 deletions photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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{
Expand All @@ -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);
Expand Down Expand Up @@ -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);
}

0 comments on commit 545e016

Please sign in to comment.