Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-06 03:31: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( std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
PhotonPipelineResult result) { PhotonPipelineResult result) {
int lowestAJ = -1;
double lowestAmbiguityScore = std::numeric_limits<double>::infinity(); double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
auto targets = result.GetTargets(); auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) { auto foundIt = targets.end();
if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) { for (auto it = targets.begin(); it != targets.end(); ++it) {
lowestAJ = j; if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
lowestAmbiguityScore = targets[j].GetPoseAmbiguity(); foundIt = it;
lowestAmbiguityScore = it->GetPoseAmbiguity();
} }
} }
if (lowestAJ == -1) { if (foundIt == targets.end()) {
return std::nullopt; return std::nullopt;
} }
PhotonTrackedTarget bestTarget = targets[lowestAJ]; auto& bestTarget = *foundIt;
std::optional<frc::Pose3d> fiducialPose = std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(bestTarget.GetFiducialId()); aprilTags.GetTagPose(bestTarget.GetFiducialId());
@@ -186,8 +186,7 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
frc::Pose3d pose = lastPose; frc::Pose3d pose = lastPose;
auto targets = result.GetTargets(); auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) { for (auto& target : targets) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose = std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId()); aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) { if (!fiducialPose) {
@@ -232,8 +231,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
double totalAmbiguity = 0; double totalAmbiguity = 0;
auto targets = result.GetTargets(); auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) { for (auto& target : targets) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose = std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId()); aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) { if (!fiducialPose) {

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

@@ -25,8 +25,6 @@
#pragma once #pragma once
#include <memory> #include <memory>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h> #include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h> #include <frc/geometry/Pose3d.h>
@@ -63,10 +61,6 @@ struct EstimatedRobotPose {
*/ */
class PhotonPoseEstimator { class PhotonPoseEstimator {
public: 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. * Create a new PhotonPoseEstimator.
* *