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

[PhotonPoseEstimator] Check for CalibData before adding Tags when using SolvePNP method (#826)

Browse Source 
Closes #825 by re-organizing multi-tag strategy order. Should be NFC


---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Sriman Achanta
2023-06-18 00:00:30 -04:00
committed by GitHub
parent 58419cfe38
commit a723d3dc5c
2 changed files with 21 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -359,11 +359,17 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
std::optional<cv::Mat> 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) {
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
}
auto const targets = result.GetTargets();
// List of corners mapped from 3d space (meters) to the 2d camera screen
@@ -393,10 +399,6 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
cv::Mat const rvec(3, 1, cv::DataType<double>::type);
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
if (!camMat || !distCoeffs) {
return std::nullopt;
}
cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
rvec, tvec, false, cv::SOLVEPNP_SQPNP);