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

6
photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
View File

@@ -25,8 +25,6 @@
#pragma once
#include <memory>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
@@ -63,10 +61,6 @@ struct EstimatedRobotPose {
*/
class PhotonPoseEstimator {
public:
using map_value_type =
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
using size_type = std::vector<map_value_type>::size_type;
/**
* Create a new PhotonPoseEstimator.
*