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

[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

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

@@ -367,11 +367,14 @@ public class PhotonPoseEstimator {
// Arrays we need declared up front
var visCorners = new ArrayList<TargetCorner>();
var knownVisTags = new ArrayList<AprilTag>();
var fieldToCams = new ArrayList<Pose3d>();
var fieldToCamsAlt = new ArrayList<Pose3d>();
if (result.getTargets().size() < 2) {
// Run fallback strategy instead
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
if (!hasCalibData) {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
@@ -388,33 +391,18 @@ public class PhotonPoseEstimator {
// actual layout poses of visible tags -- not exposed, so have to recreate
knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse()));
fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
}
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
var pnpResults =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), visCorners, knownVisTags);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
// multi-target solvePNP
if (hasCalibData) {
var cameraMatrix = cameraMatrixOpt.get();
var distCoeffs = distCoeffsOpt.get();
var pnpResults =
VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
// var alt = new Pose3d()
// .plus(pnpResults.alt) // field-to-camera
// .plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
} else {
// TODO fallback strategy? Should we just always do solvePNP?
return Optional.empty();
}
return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
}
/**

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);