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

3
photon-lib/build.gradle
View File

@@ -34,7 +34,6 @@ dependencies {
implementation wpilibTools.deps.wpilibJava("wpilibj")
implementation wpilibTools.deps.wpilibJava("apriltag")
// Junit
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
@@ -66,6 +65,7 @@ model {
}
}
nativeUtils.useRequiredLibrary(it, "wpilib_shared")
nativeUtils.useRequiredLibrary(it, "vision_shared")
}
}
testSuites {
@@ -80,6 +80,7 @@ model {
}
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
nativeUtils.useRequiredLibrary(it, "vision_shared")
nativeUtils.useRequiredLibrary(it, "googletest_static")
}
}

183
photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
View File

@@ -35,6 +35,8 @@ import edu.wpi.first.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -62,11 +64,11 @@ public class RobotPoseEstimator {
private AprilTagFieldLayout aprilTags;
private PoseStrategy strategy;
private ArrayList<Pair<PhotonCamera, Transform3d>> cameras;
private List<Pair<PhotonCamera, Transform3d>> cameras;
private Pose3d lastPose;
private Pose3d referencePose;
private HashSet<Integer> reportedErrors;
private Set<Integer> reportedErrors;
/**
* Create a new RobotPoseEstimator.
@@ -83,7 +85,7 @@ public class RobotPoseEstimator {
public RobotPoseEstimator(
AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
ArrayList<Pair<PhotonCamera, Transform3d>> cameras) {
List<Pair<PhotonCamera, Transform3d>> cameras) {
this.aprilTags = aprilTags;
this.strategy = strategy;
this.cameras = cameras;
@@ -97,37 +99,36 @@ public class RobotPoseEstimator {
* @return The updated estimated pose and the latency in milliseconds Estimated pose may be null
* if no targets were seen
*/
public Pair<Pose3d, Double> update() {
public Optional<Pair<Pose3d, Double>> update() {
if (cameras.isEmpty()) {
DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
return Pair.of(lastPose, 0.);
return Optional.empty();
}
Pair<Pose3d, Double> pair;
Pair<Pose3d, Double> pair = getResultFromActiveStrategy();
if (pair != null) {
lastPose = pair.getFirst();
}
return Optional.ofNullable(pair);
}
private Pair<Pose3d, Double> getResultFromActiveStrategy() {
switch (strategy) {
case LOWEST_AMBIGUITY:
pair = lowestAmbiguityStrategy();
lastPose = pair.getFirst();
return pair;
return lowestAmbiguityStrategy();
case CLOSEST_TO_CAMERA_HEIGHT:
pair = closestToCameraHeightStrategy();
lastPose = pair.getFirst();
return pair;
return closestToCameraHeightStrategy();
case CLOSEST_TO_REFERENCE_POSE:
pair = closestToReferencePoseStrategy();
lastPose = pair.getFirst();
return pair;
return closestToReferencePoseStrategy();
case CLOSEST_TO_LAST_POSE:
referencePose = lastPose;
pair = closestToReferencePoseStrategy();
lastPose = pair.getFirst();
return pair;
return closestToLastPoseStrategy();
case AVERAGE_BEST_TARGETS:
pair = averageBestTargetsStrategy();
lastPose = pair.getFirst();
return pair;
return averageBestTargetsStrategy();
default:
DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
return Pair.of(null, 0.);
return null;
}
}
@@ -157,37 +158,29 @@ public class RobotPoseEstimator {
// No targets, return null
if (lowestAI == -1 || lowestAJ == -1) {
return Pair.of(null, 0.);
return null;
}
// Pick the lowest and do the heavy calculations
PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
// If the map doesn't contain the ID fail
var tmp = aprilTags.getTagPose(bestTarget.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(bestTarget.getFiducialId())) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ bestTarget.getFiducialId(),
false);
reportedErrors.add(bestTarget.getFiducialId());
}
return Pair.of(lastPose, 0.);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(bestTarget.getFiducialId());
return null;
}
var tagPose = tmp.get();
return Pair.of(
tagPose
fiducialPose
.get()
.transformBy(bestTarget.getBestCameraToTarget().inverse())
.transformBy(cameras.get(lowestAI).getSecond().inverse()),
results.get(lowestAI).getLatencyMillis());
}
private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
double smallestHeightDifference = 10e9;
double mili = 0;
double smallestHeightDifference = Double.MAX_VALUE;
double latency = 0;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
@@ -196,18 +189,12 @@ public class RobotPoseEstimator {
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
var tmp = aprilTags.getTagPose(target.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(target.getFiducialId())) {
DriverStation.reportWarning(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ target.getFiducialId(),
false);
reportedErrors.add(target.getFiducialId());
}
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = tmp.get();
Pose3d targetPose = fiducialPose.get();
double alternativeDifference =
Math.abs(
p.getSecond().getZ()
@@ -222,7 +209,7 @@ public class RobotPoseEstimator {
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
mili = result.getLatencyMillis();
latency = result.getLatencyMillis();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
@@ -230,12 +217,12 @@ public class RobotPoseEstimator {
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
mili = result.getLatencyMillis();
latency = result.getLatencyMillis();
}
}
}
return Pair.of(pose, mili);
return Pair.of(pose, latency);
}
private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
@@ -243,10 +230,10 @@ public class RobotPoseEstimator {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
false);
return Pair.of(null, 0.);
return null;
}
double smallestDifference = 10e9;
double mili = 0;
double latency = 0;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
@@ -254,19 +241,12 @@ public class RobotPoseEstimator {
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
// If the map doesn't contain the ID fail
var tmp = aprilTags.getTagPose(target.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(target.getFiducialId())) {
DriverStation.reportWarning(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ target.getFiducialId(),
false);
reportedErrors.add(target.getFiducialId());
}
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = tmp.get();
Pose3d targetPose = fiducialPose.get();
Pose3d botBestPose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
@@ -280,16 +260,21 @@ public class RobotPoseEstimator {
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = botAltPose;
mili = result.getLatencyMillis();
latency = result.getLatencyMillis();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = botBestPose;
mili = result.getLatencyMillis();
latency = result.getLatencyMillis();
}
}
}
return Pair.of(pose, mili);
return Pair.of(pose, latency);
}
private Pair<Pose3d, Double> closestToLastPoseStrategy() {
setReferencePose(lastPose);
return closestToReferencePoseStrategy();
}
/** Return the average of the best target poses using ambiguity as weight */
@@ -303,19 +288,12 @@ public class RobotPoseEstimator {
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
// If the map doesn't contain the ID fail
var tmp = aprilTags.getTagPose(target.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(target.getFiducialId())) {
DriverStation.reportWarning(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ target.getFiducialId(),
false);
reportedErrors.add(target.getFiducialId());
}
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = tmp.get();
Pose3d targetPose = fiducialPose.get();
try {
totalAmbiguity += 1. / target.getPoseAmbiguity();
} catch (ArithmeticException e) {
@@ -339,25 +317,24 @@ public class RobotPoseEstimator {
Rotation3d rotation = new Rotation3d();
double latency = 0;
if (tempPoses.size() > 0) {
for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
try {
double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
transform = transform.plus(pair.getFirst().getTranslation().times(weight));
rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
latency +=
pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
} catch (ArithmeticException e) {
DriverStation.reportWarning(
"[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!",
false);
return Pair.of(pair.getFirst(), pair.getSecond().getSecond());
}
}
return Pair.of(new Pose3d(transform, rotation), latency);
} else {
return new Pair<Pose3d, Double>(null, -1.0);
if (tempPoses.isEmpty()) {
return null;
}
if (totalAmbiguity == 0) {
Pose3d p = tempPoses.get(0).getFirst();
double l = tempPoses.get(0).getSecond().getSecond();
return Pair.of(p, l);
}
for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
transform = transform.plus(pair.getFirst().getTranslation().times(weight));
rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
}
return Pair.of(new Pose3d(transform, rotation), latency);
}
/**
@@ -409,7 +386,7 @@ public class RobotPoseEstimator {
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
this.referencePose = new Pose3d(referencePose);
setReferencePose(new Pose3d(referencePose));
}
/**
@@ -420,4 +397,12 @@ public class RobotPoseEstimator {
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
private void reportFiducialPoseError(int fiducialId) {
if (!reportedErrors.contains(fiducialId)) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: " + fiducialId, false);
reportedErrors.add(fiducialId);
}
}
}

9
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
View File

@@ -27,7 +27,6 @@ package org.photonvision;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
@@ -139,13 +138,7 @@ public class SimVisionSystem {
* PhotonVision parameters.
*/
public void processFrame(Pose2d robotPoseMeters) {
var robotPose3d =
new Pose3d(
robotPoseMeters.getX(),
robotPoseMeters.getY(),
0.0,
new Rotation3d(0, 0, robotPoseMeters.getRotation().getRadians()));
processFrame(robotPose3d);
processFrame(new Pose3d(robotPoseMeters));
}
/**

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;

33
photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java
View File

@@ -42,6 +42,7 @@ import edu.wpi.first.util.WPIUtilJNI;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.RobotPoseEstimator.PoseStrategy;
@@ -138,10 +139,10 @@ class RobotPoseEstimatorTest {
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.getSecond());
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(1, pose.getX(), .01);
assertEquals(3, pose.getY(), .01);
assertEquals(2, pose.getZ(), .01);
@@ -210,10 +211,10 @@ class RobotPoseEstimatorTest {
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.getSecond());
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);
@@ -283,10 +284,10 @@ class RobotPoseEstimatorTest {
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(4, estimatedPose.getSecond());
assertEquals(4, estimatedPose.get().getSecond());
assertEquals(1, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(.9, pose.getZ(), .01);
@@ -357,8 +358,8 @@ class RobotPoseEstimatorTest {
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
cameraOne.result =
new PhotonPipelineResult(
@@ -412,9 +413,9 @@ class RobotPoseEstimatorTest {
new TargetCorner(7, 8)))));
estimatedPose = estimator.update();
pose = estimatedPose.getFirst();
pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.getSecond());
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(.9, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(1, pose.getZ(), .01);
@@ -483,9 +484,9 @@ class RobotPoseEstimatorTest {
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
assertEquals(2.6885245901639347, estimatedPose.getSecond(), .01);
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
assertEquals(2.15, pose.getX(), .01);
assertEquals(2.15, pose.getY(), .01);
assertEquals(2.15, pose.getZ(), .01);

64
photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
View File

@@ -26,6 +26,7 @@
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
@@ -40,11 +41,14 @@
#include "photonlib/RobotPoseEstimator.h"
TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -122,11 +126,14 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
}
TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -204,11 +211,14 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
}
TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -288,11 +298,13 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
}
TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -431,11 +443,13 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
}
TEST(RobotPoseEstimatorTest, AverageBestPoses) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>