Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +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