Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-25 01:41:40 +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!",

16
photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
View File

@@ -34,6 +34,10 @@
#include "photonlib/PhotonCamera.h"
namespace frc {
class AprilTagFieldLayout;
} // namespace frc
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
@@ -47,18 +51,18 @@ enum PoseStrategy : int {
* A managing class to determine how an estimated pose should be chosen.
*/
class RobotPoseEstimator {
public:
using map_value_type =
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
using size_type = std::vector<map_value_type>::size_type;
public:
explicit RobotPoseEstimator(std::map<int, frc::Pose3d> aprilTags,
PoseStrategy strategy,
std::vector<map_value_type>);
explicit RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
PoseStrategy strategy, std::vector<map_value_type> cameras);
std::pair<frc::Pose3d, units::millisecond_t> Update();
void SetPoseStrategy(PoseStrategy strategy);
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
@@ -79,7 +83,7 @@ class RobotPoseEstimator {
frc::Pose3d GetReferencePose() const { return referencePose; }
private:
std::map<int, frc::Pose3d> aprilTags;
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
PoseStrategy strategy;
std::vector<map_value_type> cameras;
frc::Pose3d lastPose;