Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-27 02:01:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix fallback for the multitag on rio pose strategy (#1755)

Browse Source
This commit is contained in:
Joseph Eng
2025-02-01 14:09:43 -08:00
committed by GitHub
parent 8ec2da952f
commit 99427d888a
5 changed files with 184 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -286,8 +286,8 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
* provided
* Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not
* provided in this overload.
*
* @param cameraResult The latest pipeline result from the camera
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
@@ -341,6 +341,20 @@ public class PhotonPoseEstimator {
return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy);
}
/**
* Internal convenience method for using a fallback strategy for update(). This should only be
* called after timestamp checks have been done by another update() overload.
*
* @param cameraResult The latest pipeline result from the camera
* @param strategy The pose strategy to use
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult, PoseStrategy strategy) {
return update(cameraResult, Optional.empty(), Optional.empty(), strategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
@@ -357,21 +371,8 @@ public class PhotonPoseEstimator {
yield closestToReferencePoseStrategy(cameraResult, referencePose);
}
case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
case MULTI_TAG_PNP_ON_RIO -> {
if (cameraMatrix.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
yield Optional.empty();
} else if (distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
yield Optional.empty();
} else {
yield multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
}
}
case MULTI_TAG_PNP_ON_RIO ->
multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
};
@@ -383,41 +384,44 @@ public class PhotonPoseEstimator {
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().isPresent()) {
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
.relativeTo(fieldTags.getOrigin())
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
// We can never fall back on another multitag strategy
return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
if (result.getMultiTagResult().isEmpty()) {
return update(result, this.multiTagFallbackStrategy);
}
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
.relativeTo(fieldTags.getOrigin())
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
}
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData || result.getTargets().size() < 2) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
return update(result, this.multiTagFallbackStrategy);
}
if (result.getTargets().size() < 2) {
return update(result, this.multiTagFallbackStrategy);
}
var pnpResult =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
if (!pnpResult.isPresent()) return update(result, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResult.get().best) // field-to-camera

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

@@ -350,18 +350,17 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (result.MultiTagResult()) {
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
if (!result.MultiTagResult()) {
return Update(result, this->multiTagFallbackStrategy);
}
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
@@ -370,19 +369,17 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
using namespace frc;
// Need at least 2 targets
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
}
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"PhotonPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
return Update(result, this->multiTagFallbackStrategy);
}
// Need at least 2 targets
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
}
auto const targets = result.GetTargets();
@@ -408,7 +405,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
// We should only do multi-tag if at least 2 tags (* 4 corners/tag)
if (imagePoints.size() < 8) {
return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy);
return Update(result, this->multiTagFallbackStrategy);
}
// Output mats for results

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

@@ -204,6 +204,21 @@ class PhotonPoseEstimator {
inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
/**
* Internal convenience method for using a fallback strategy for update().
* This should only be called after timestamp checks have been done by another
* update() overload.
*
* @param cameraResult The latest pipeline result from the camera
* @param strategy The pose strategy to use
* @return an EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
PoseStrategy strategy) {
return Update(result, std::nullopt, std::nullopt, strategy);
}
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,

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

@@ -24,6 +24,7 @@
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -645,6 +646,72 @@ class PhotonPoseEstimatorTest {
assertEquals(2.15, pose.getZ(), .01);
}
@Test
void testMultiTagOnRioFallback() {
PhotonCameraInjector camera = new PhotonCameraInjector();
camera.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
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))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
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)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(camera.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");

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

@@ -419,6 +419,49 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};