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

[photonlib] Remove deprecated pose estimator methods (#2431)

Browse Source 
We added new API methods in 2026, and deprecated our past methods. This PR removes the deprecated methods.
This commit is contained in:
Sam Freund
2026-04-13 12:04:58 -05:00
committed by GitHub
parent 68fc1e7129
commit e970446c4c
6 changed files with 2 additions and 2330 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
docs/source/docs/apriltag-pipelines/multitag.md
View File

@@ -23,7 +23,7 @@ Ensure that your camera is calibrated and 3D mode is enabled. Navigate to the Ou
By default, enabling multi-target will disable calculating camera-to-target transforms for each observed AprilTag target to increase performance; the X/Y/angle numbers shown in the target table of the UI are instead calculated using the tag's expected location (per the field layout JSON) and the field-to-camera transform calculated using MultiTag. If you additionally want the individual camera-to-target transform calculated using SolvePNP for each target, enable "Always Do Single-Target Estimation".
:::
This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class <docs/programming/photonlib/robot-pose-estimator:AprilTags and PhotonPoseEstimator>` with the `MULTI_TAG_PNP_ON_COPROCESSOR` strategy to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()`/`multitagResult` (Java/C++/Python).
This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class <docs/programming/photonlib/robot-pose-estimator:AprilTags and PhotonPoseEstimator>` and calling `estimateCoprocMultiTagPose` (with an optional fallback like `estimateLowestAmbiguityPose`) to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()`/`multitagResult` (Java/C++/Python).
```{eval-rst}
.. tab-set-code::

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

@@ -45,7 +45,6 @@ import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N8;
import org.wpilib.math.util.Pair;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.camera.OpenCvLoader;
/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
@@ -123,13 +122,7 @@ public class PhotonPoseEstimator {
private AprilTagFieldLayout fieldTags;
private TargetModel tagModel = TargetModel.kAprilTag36h11;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private Transform3d robotToCamera;
private Pose3d lastPose;
private Pose3d referencePose;
protected double poseCacheTimestampSeconds = -1;
private final Set<Integer> reportedErrors = new HashSet<>();
private final TimeInterpolatableBuffer<Rotation2d> headingBuffer =
@@ -156,48 +149,6 @@ public class PhotonPoseEstimator {
InstanceCount++;
}
/**
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
* with respect to the FIRST field using the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
* @deprecated Use individual estimation methods with the 2 argument constructor instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.primaryStrategy = strategy;
this.robotToCamera = robotToCamera;
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO
|| strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) {
OpenCvLoader.forceStaticLoad();
}
InstanceCount++;
HAL.reportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
}
private void checkUpdate(Object oldObj, Object newObj) {
if (!Objects.equals(oldObj, newObj)) {
invalidatePoseCache();
}
}
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
@@ -217,7 +168,6 @@ public class PhotonPoseEstimator {
* @param fieldTags the AprilTagFieldLayout
*/
public void setFieldTags(AprilTagFieldLayout fieldTags) {
checkUpdate(this.fieldTags, fieldTags);
this.fieldTags = fieldTags;
}
@@ -239,113 +189,6 @@ public class PhotonPoseEstimator {
this.tagModel = tagModel;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PoseStrategy getPrimaryStrategy() {
return primaryStrategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setPrimaryStrategy(PoseStrategy strategy) {
checkUpdate(this.primaryStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO
|| strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) {
OpenCvLoader.forceStaticLoad();
}
this.primaryStrategy = strategy;
}
/**
* Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
* NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
|| strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
}
this.multiTagFallbackStrategy = strategy;
}
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Pose3d getReferencePose() {
return referencePose;
}
/**
* Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose3d referencePose) {
checkUpdate(this.referencePose, referencePose);
this.referencePose = referencePose;
}
/**
* Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose2d referencePose) {
setReferencePose(new Pose3d(referencePose));
}
/**
* Update the stored last pose. Useful for setting the initial estimate when using the
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when using the
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose2d lastPose) {
setLastPose(new Pose3d(lastPose));
}
/**
* Add robot heading data to buffer. Must be called periodically for the
* <b>PNP_DISTANCE_TRIG_SOLVE</b> strategy.
@@ -413,253 +256,6 @@ public class PhotonPoseEstimator {
this.robotToCamera = robotToCamera;
}
/**
* Updates the estimated position of the robot, assuming no camera calibration is required for the
* selected strategy. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* </ul>
*
* 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
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
return update(cameraResult, Optional.empty(), Optional.empty());
}
/**
* Updates the estimated position of the robot. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* <li>The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the
* other function overload).
* </ul>
*
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs) {
return update(cameraResult, cameraMatrix, distCoeffs, Optional.empty());
}
/**
* Updates the estimated position of the robot. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* <li>The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty.
* </ul>
*
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
// 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();
}
return update(
cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, 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. Can't be CONSTRAINED_SOLVEPNP.
* @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(), Optional.empty(), strategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy) {
Optional<EstimatedRobotPose> estimatedPose =
switch (strategy) {
case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult);
case CLOSEST_TO_REFERENCE_POSE ->
estimateClosestToReferencePose(cameraResult, referencePose);
case CLOSEST_TO_LAST_POSE -> {
setReferencePose(lastPose);
yield estimateClosestToReferencePose(cameraResult, referencePose);
}
case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult);
case MULTI_TAG_PNP_ON_RIO -> {
if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
yield update(cameraResult, this.multiTagFallbackStrategy);
}
var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get());
if (res.isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
constrainedPnpParams,
this.multiTagFallbackStrategy);
}
yield res;
}
case MULTI_TAG_PNP_ON_COPROCESSOR -> {
if (cameraResult.getMultiTagResult().isEmpty()) {
yield update(cameraResult, this.multiTagFallbackStrategy);
}
yield estimateCoprocMultiTagPose(cameraResult);
}
case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult);
case CONSTRAINED_SOLVEPNP -> {
boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
yield Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (cameraResult.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
cameraResult
.getMultiTagResult()
.get()
.estimatedPose
.best
.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
yield Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
}
var pnpResult =
estimateConstrainedSolvepnpPose(
cameraResult,
cameraMatrix.get(),
distCoeffs.get(),
fieldToRobotSeed,
constrainedPnpParams.get().headingFree,
constrainedPnpParams.get().headingScaleFactor);
if (!pnpResult.isPresent()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
yield pnpResult;
}
};
if (estimatedPose.isPresent()) {
lastPose = estimatedPose.get().estimatedPose;
}
return estimatedPose;
}
/**
* @param cameraResult A pipeline result from the camera.
* @return Whether or not pose estimation should be performed.

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

@@ -41,7 +41,6 @@
#include <wpi/units/angle.hpp>
#include <wpi/units/math.hpp>
#include <wpi/units/time.hpp>
#include <wpi/util/deprecated.hpp>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
@@ -51,7 +50,6 @@
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
#include <opencv2/core/eigen.hpp>
WPI_IGNORE_DEPRECATED
namespace photon {
namespace detail {
@@ -69,186 +67,12 @@ PhotonPoseEstimator::PhotonPoseEstimator(
wpi::math::Transform3d robotToCamera)
: aprilTags(tags),
m_robotToCamera(robotToCamera),
lastPose(wpi::math::Pose3d()),
referencePose(wpi::math::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
InstanceCount++;
}
PhotonPoseEstimator::PhotonPoseEstimator(
wpi::apriltag::AprilTagFieldLayout tags, PoseStrategy strat,
wpi::math::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
m_robotToCamera(robotToCamera),
lastPose(wpi::math::Pose3d()),
referencePose(wpi::math::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
InstanceCount++;
HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
strategy == MULTI_TAG_PNP_ON_RIO) {
WPILIB_ReportError(
wpi::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
"");
strategy = LOWEST_AMBIGUITY;
}
if (this->multiTagFallbackStrategy != strategy) {
InvalidatePoseCache();
}
multiTagFallbackStrategy = strategy;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
WPILIB_ReportError(wpi::warn::Warning,
"Result timestamp was reported in the past!");
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 &&
wpi::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;
}
return Update(result, cameraMatrixData, cameraDistCoeffs,
constrainedPnpParams, this->strategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy) {
std::optional<EstimatedRobotPose> ret = std::nullopt;
switch (strategy) {
case LOWEST_AMBIGUITY:
ret = EstimateLowestAmbiguityPose(result);
break;
case CLOSEST_TO_CAMERA_HEIGHT:
ret = EstimateClosestToCameraHeightPose(result);
break;
case CLOSEST_TO_REFERENCE_POSE:
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case AVERAGE_BEST_TARGETS:
ret = EstimateAverageBestTargetsPose(result);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
if (!result.MultiTagResult()) {
ret = Update(result, this->multiTagFallbackStrategy);
} else {
ret = EstimateCoprocMultiTagPose(result);
}
break;
case MULTI_TAG_PNP_ON_RIO:
if (!cameraMatrixData && !cameraDistCoeffs) {
WPILIB_ReportError(
wpi::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!", "");
ret = Update(result, this->multiTagFallbackStrategy);
}
ret =
EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs);
if (!ret) {
ret = Update(result, this->multiTagFallbackStrategy);
}
break;
case CONSTRAINED_SOLVEPNP: {
if (!cameraMatrixData || !cameraDistCoeffs) {
WPILIB_ReportError(
wpi::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
break;
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
break;
}
wpi::math::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
wpi::math::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
ret = EstimateConstrainedSolvepnpPose(
result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed,
constrainedPnpParams->headingFree,
constrainedPnpParams->headingScalingFactor);
if (!ret) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
}
break;
}
case PNP_DISTANCE_TRIG_SOLVE:
ret = EstimatePnpDistanceTrigSolvePose(result);
break;
default:
WPILIB_ReportError(wpi::warn::Warning, "Invalid Pose Strategy selected!",
"");
ret = std::nullopt;
}
if (ret) {
lastPose = ret->estimatedPose;
}
return ret;
}
bool ShouldEstimate(const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
@@ -359,7 +183,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose(
wpi::units::meter_t smallestDifference =
wpi::units::meter_t(std::numeric_limits<double>::infinity());
wpi::units::second_t stateTimestamp = wpi::units::second_t(0);
wpi::math::Pose3d pose = lastPose;
wpi::math::Pose3d pose;
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {

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

@@ -97,24 +97,6 @@ class PhotonPoseEstimator {
explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags,
wpi::math::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
* @deprecated Use individual estimation methods with the 2 argument
* constructor instead.
*/
[[deprecated(
"Use individual estimation methods with the 2 argument constructor "
"instead.")]]
explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
wpi::math::Transform3d robotToCamera);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
@@ -124,67 +106,6 @@ class PhotonPoseEstimator {
return aprilTags;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
PoseStrategy GetPoseStrategy() const {
return strategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetPoseStrategy(PoseStrategy strat) {
if (strategy != strat) {
InvalidatePoseCache();
}
strategy = strat;
}
/**
* Set the Position Estimation Strategy used in multi-tag mode when
* only one tag can be seen. Must NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
void SetMultiTagFallbackStrategy(PoseStrategy strategy);
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
wpi::math::Pose3d GetReferencePose() const {
return referencePose;
}
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetReferencePose(wpi::math::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
}
this->referencePose = referencePose;
}
/**
* @return The current transform from the center of the robot to the camera
* mount position.
@@ -203,18 +124,6 @@ class PhotonPoseEstimator {
m_robotToCamera = robotToCamera;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetLastPose(wpi::math::Pose3d lastPose) {
this->lastPose = lastPose;
}
/**
* Add robot heading data to the buffer. Must be called periodically for the
* PNP_DISTANCE_TRIG_SOLVE strategy.
@@ -270,29 +179,6 @@ class PhotonPoseEstimator {
ResetHeadingData(timestamp, heading.ToRotation2d());
}
/**
* Update the pose estimator. If updating multiple times per loop, you should
* call this exactly once per new result, in order of increasing result
* timestamp.
*
* @param result The vision targeting result to process
* @param cameraIntrinsics The camera calibration pinhole coefficients matrix.
* Only required if doing multitag-on-rio, and may be nullopt otherwise.
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
std::optional<EstimatedRobotPose> Update(
const photon::PhotonPipelineResult& result,
std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
std::nullopt,
std::optional<photon::PhotonCamera::DistortionMatrix> coeffsData =
std::nullopt,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
std::nullopt);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
@@ -420,43 +306,12 @@ class PhotonPoseEstimator {
private:
wpi::apriltag::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
wpi::math::Transform3d m_robotToCamera;
wpi::math::Pose3d lastPose;
wpi::math::Pose3d referencePose;
wpi::units::second_t poseCacheTimestamp;
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d> headingBuffer;
inline static int InstanceCount = 1;
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 A 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, std::nullopt, strategy);
}
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> coeffsData,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy);
};
} // namespace photon

936
photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java
View File

@@ -1,936 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
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.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineMetadata;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Quaternion;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.Nat;
import org.wpilib.math.util.Units;
import org.wpilib.util.runtime.RuntimeLoader;
import org.wpilib.vision.apriltag.AprilTag;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
class LegacyPhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@AutoClose final PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@BeforeAll
public static void init() throws IOException {
if (!LibraryLoader.loadWpiLibraries()) {
fail();
}
RuntimeLoader.loadLibrary("photontargetingJNI");
HAL.initialize(1000, 0);
List<AprilTag> tagList = new ArrayList<>(2);
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
double fieldLength = Units.feetToMeters(54.0);
double fieldWidth = Units.feetToMeters(27.0);
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
}
@AfterAll
public static void teardown() {
HAL.shutdown();
}
@Test
void testLowestAmbiguityStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
11 * 1000000,
1100000,
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))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.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.4,
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.LOWEST_AMBIGUITY, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(3, pose.getY(), .01);
assertEquals(2, pose.getZ(), .01);
}
@Test
void testClosestToCameraHeightStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
4000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), 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))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
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))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
0.4,
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.CLOSEST_TO_CAMERA_HEIGHT,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);
}
@Test
void closestToReferencePoseStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
17000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), 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))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
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))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
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.CLOSEST_TO_REFERENCE_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(.9, pose.getZ(), .01);
}
@Test
void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), 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))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
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))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
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.CLOSEST_TO_LAST_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
new PhotonPipelineResult(
0,
7000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), 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))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
0,
-1,
-1,
new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
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))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
0.4,
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)))));
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
assertEquals(.9, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(1, pose.getZ(), .01);
}
@Test
void pnpDistanceTrigSolve() {
List<VisionTargetSim> simTargets =
aprilTags.getTags().stream()
.map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
.toList();
try (PhotonCameraSim cameraOneSim =
new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
/* Compound Rolled + Pitched + Yaw */
Transform3d compoundTestTransform =
new Transform3d(
-Units.inchesToMeters(12),
-Units.inchesToMeters(11),
3,
new Rotation3d(
Units.degreesToRadians(37),
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
PhotonPipelineResult result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
var bestTarget = result.getBestTarget();
assertNotNull(bestTarget);
assertEquals(0, bestTarget.fiducialId);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
/* Straight on */
Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);
estimator.setRobotToCameraTransform(straightOnTestTransform);
/* Pose to compare with */
realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
}
}
@Test
void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
20_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(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)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
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(cameraOne.result);
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(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
cameraOne.result =
new PhotonPipelineResult(
0,
20 * 1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
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))), // 1 1 1 ambig: .7
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
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))), // 2 2 2 ambig .3
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
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))))); // 3 3 3 ambig .4
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(2.15, pose.getX(), .01);
assertEquals(2.15, pose.getY(), .01);
assertEquals(2.15, pose.getZ(), .01);
}
@Test
void testMultiTagOnRioFallback() {
cameraOne.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(cameraOne.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));
}
@Test
public void testConstrainedPnpOneTag() {
var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
var cameraMat =
MatBuilder.fill(
Nat.N3(),
Nat.N3(),
399.37500000000006,
0,
319.5,
0,
399.16666666666674,
239.5,
0,
0,
1);
/*
* Ground truth:
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0.
* 31064262190452635
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0.
* 24478552235412665
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0.
* 0836470779150917
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0.
* 914649865171567
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3.
* 191446451763934
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4.
* 44396966389316
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0.
* 4995793771070878
*/
List<TargetCorner> corners8 =
List.of(
new TargetCorner(98.09875447066685, 331.0093220119495),
new TargetCorner(122.20226758624413, 335.50083894738486),
new TargetCorner(127.17118732489361, 313.81406314178633),
new TargetCorner(104.28543773760417, 309.6516557438994));
var result =
new PhotonPipelineResult(
new PhotonPipelineMetadata(10000, 2000, 1, 100),
List.of(
new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
Optional.of(
new MultiTargetPNPResult(
new PnpResult(
new Transform3d(
// From ground truth
new Translation3d(
3.1665557336121353, 4.430673446050584, 0.48678786477534686),
new Rotation3d(
new Quaternion(
0.3132532247418243,
0.24722671090692333,
-0.08413452932300695,
0.9130568172784148))),
0.1),
new ArrayList<Short>(8))));
final double camPitch = Units.degreesToRadians(30.0);
final Transform3d kRobotToCam =
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");
}
PhotonPipelineResult result;
@Override
public List<PhotonPipelineResult> getAllUnreadResults() {
return List.of(result);
}
@Override
public PhotonPipelineResult getLatestResult() {
return result;
}
}
}

667
photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp
View File

@@ -1,667 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <map>
#include <utility>
#include <vector>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/length.hpp>
#include <wpi/util/SmallVector.hpp>
#include <wpi/util/deprecated.hpp>
#include "gtest/gtest.h"
#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/simulation/PhotonCameraSim.h"
#include "photon/simulation/SimCameraProperties.h"
#include "photon/simulation/VisionTargetSim.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
WPI_IGNORE_DEPRECATED
namespace units = wpi::units;
static std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), wpi::math::Rotation3d())}};
static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
static std::vector<photon::TargetCorner> detectedCorners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
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,
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::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,
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, 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,
wpi::math::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
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(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), wpi::math::Rotation3d())},
};
auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photon::PhotonCamera, wpi::math::Transform3d>> cameras;
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
// ID 0 at 3,3,3
// ID 1 at 5,5,5
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, 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(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(wpi::math::Pose3d(
1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, 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(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(wpi::math::Pose3d(
1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
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);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
cameraOne.test = true;
std::vector<photon::VisionTargetSim> targets;
targets.reserve(tags.size());
for (const auto& tag : tags) {
targets.push_back(
photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID));
}
photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim(
&cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
/* Compound Rolled + Pitched + Yaw */
wpi::math::Transform3d compoundTestTransform = wpi::math::Transform3d(
-12_in, -11_in, 3_m, wpi::math::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* real pose of the robot base to test against */
wpi::math::Pose3d realPose = wpi::math::Pose3d(
7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d(
0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m,
wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(18_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
estimatedPose = std::nullopt;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
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,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, 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(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
// empty input, expect empty out
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// If the camera produces a result that is > 1 micro second later,
// the pose cache should not be hit.
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(
wpi::math::Pose3d(units::meter_t(1), units::meter_t(2), units::meter_t(3),
wpi::math::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
}
TEST(LegacyPhotonPoseEstimatorTest, 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,
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::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,
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::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,
wpi::math::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
wpi::math::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(LegacyPhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};
auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
testResult.SetReceiveTimestamp(units::second_t(11));
auto test2 = testResult;
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
{0, 399.16666666666674, 239.5},
{0, 0, 1}};
// Create corners data matching the Java test
std::vector<photon::TargetCorner> corners8{
photon::TargetCorner{98.09875447066685, 331.0093220119495},
photon::TargetCorner{122.20226758624413, 335.50083894738486},
photon::TargetCorner{127.17118732489361, 313.81406314178633},
photon::TargetCorner{104.28543773760417, 309.6516557438994}};
wpi::math::Transform3d poseTransform(
wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
wpi::math::Rotation3d(
wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695, 0.9130568172784148)));
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
poseTransform, 0.0, corners8, corners8}};
auto multiTagResult = std::make_optional<photon::MultiTargetPNPResult>(
photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0},
std::vector<int16_t>{8});
photon::PhotonPipelineResult result{
photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
multiTagResult};
cameraOne.test = true;
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
const units::radian_t camPitch = 30_deg;
const wpi::math::Transform3d kRobotToCam{
wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m),
wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
wpi::math::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
photon::ConstrainedSolvepnpParams{true, 0});
ASSERT_TRUE(estimatedPose.has_value());
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
}