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

RobotPoseEstimator Enhancements (#677)

Browse Source 
* Use List for RobotPoseEstimator constructor

* Update `RobotPoseEstimator` constructor to accept wpilib `AprilTagFieldLayout` java

* Initial cpp changes

* Java return optional from update

* Fix java test

* Clean up strategy switch

* small lint

* Actually link to vision_shared

* Fix auto optimized imports

* format

* report error

* small method changes

* format and clean up

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Nick Hadley
2023-01-02 19:22:39 -05:00
committed by GitHub
parent 971b471f92
commit 0b5256df12
7 changed files with 171 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
View File

@@ -33,6 +33,7 @@
#include <vector>
#include <frc/Errors.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
@@ -44,7 +45,7 @@
namespace photonlib {
RobotPoseEstimator::RobotPoseEstimator(
std::map<int, frc::Pose3d> tags, PoseStrategy strat,
std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
cams)
: aprilTags(tags),
@@ -112,7 +113,9 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
PhotonTrackedTarget bestTarget =
cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
if (aprilTags.count(bestTarget.GetFiducialId()) == 0) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(bestTarget.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
@@ -120,7 +123,7 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
}
return std::make_pair(
aprilTags[bestTarget.GetFiducialId()]
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(cameras[lowestAI].second.Inverse()),
cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.);
@@ -138,13 +141,15 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
if (aprilTags.count(target.GetFiducialId()) == 0) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
@@ -180,13 +185,15 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
if (aprilTags.count(target.GetFiducialId()) == 0) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose
@@ -225,14 +232,16 @@ RobotPoseEstimator::AverageBestTargetsStrategy() {
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
if (aprilTags.count(target.GetFiducialId()) == 0) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
frc::Pose3d targetPose = fiducialPose.value();
if (target.GetPoseAmbiguity() == 0) {
FRC_ReportError(frc::warn::Warning,
"Pose ambiguity of zero exists, using that instead!",