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: Stop manually iterating targets (#796)

Browse Source
This commit is contained in:
David Vo
2023-02-11 23:44:22 +11:00
committed by GitHub
parent b4c93e5d34
commit 3edc8750ec
2 changed files with 9 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -98,21 +98,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
PhotonPipelineResult result) {
int lowestAJ = -1;
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
lowestAJ = j;
lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
auto foundIt = targets.end();
for (auto it = targets.begin(); it != targets.end(); ++it) {
if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
foundIt = it;
lowestAmbiguityScore = it->GetPoseAmbiguity();
}
}
if (lowestAJ == -1) {
if (foundIt == targets.end()) {
return std::nullopt;
}
PhotonTrackedTarget bestTarget = targets[lowestAJ];
auto& bestTarget = *foundIt;
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(bestTarget.GetFiducialId());
@@ -186,8 +186,7 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
frc::Pose3d pose = lastPose;
auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
@@ -232,8 +231,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
double totalAmbiguity = 0;
auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {