Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-26 01:51: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

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,