reportedErrors;
+
+ /**
+ * Create a new RobotPoseEstimator.
+ *
+ * Example: {@code Map map = new HashMap<>(); map.put(1, new
+ * Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0)
}
+ *
+ * @param aprilTags A Map linking AprilTag IDs to Pose3ds with respect to the FIRST field.
+ * @param strategy The strategy it should use to determine the best pose.
+ * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
+ * the center of the robot to the cameras.
+ */
+ public RobotPoseEstimator(
+ Map aprilTags,
+ PoseStrategy strategy,
+ ArrayList> cameras) {
+ this.aprilTags = aprilTags;
+ this.strategy = strategy;
+ this.cameras = cameras;
+ lastPose = new Pose3d();
+ reportedErrors = new HashSet<>();
+ }
+
+ /**
+ * Update the estimated pose using the selected strategy.
+ *
+ * @return The updated estimated pose and the latency in milliseconds
+ */
+ public Pair update() {
+ if (cameras.isEmpty()) {
+ DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
+ return Pair.of(lastPose, 0.);
+ }
+ Pair pair;
+ switch (strategy) {
+ case LOWEST_AMBIGUITY:
+ pair = lowestAmbiguityStrategy();
+ lastPose = pair.getFirst();
+ return pair;
+ case CLOSEST_TO_CAMERA_HEIGHT:
+ pair = closestToCameraHeightStrategy();
+ lastPose = pair.getFirst();
+ return pair;
+ case CLOSEST_TO_REFERENCE_POSE:
+ pair = closestToReferencePoseStrategy();
+ lastPose = pair.getFirst();
+ return pair;
+ case CLOSEST_TO_LAST_POSE:
+ referencePose = lastPose;
+ pair = closestToReferencePoseStrategy();
+ lastPose = pair.getFirst();
+ return pair;
+ case AVERAGE_BEST_TARGETS:
+ pair = averageBestTargetsStrategy();
+ lastPose = pair.getFirst();
+ return pair;
+ default:
+ DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
+ return Pair.of(lastPose, 0.);
+ }
+ }
+
+ private Pair lowestAmbiguityStrategy() {
+ // Loop over each ambiguity of all the cameras
+ int lowestAI = -1;
+ int lowestAJ = -1;
+ double lowestAmbiguityScore = 10;
+ for (int i = 0; i < cameras.size(); i++) {
+ Pair p = cameras.get(i);
+ List targets = p.getFirst().getLatestResult().targets;
+ for (int j = 0; j < targets.size(); j++) {
+ if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
+ lowestAI = i;
+ lowestAJ = j;
+ lowestAmbiguityScore = targets.get(j).getPoseAmbiguity();
+ }
+ }
+ }
+
+ // No targets, return the last pose
+ if (lowestAI == -1 || lowestAJ == -1) {
+ return Pair.of(lastPose, 0.);
+ }
+
+ // Pick the lowest and do the heavy calculations
+ PhotonTrackedTarget bestTarget =
+ cameras.get(lowestAI).getFirst().getLatestResult().targets.get(lowestAJ);
+
+ // If the map doesn't contain the ID fail
+ if (!aprilTags.containsKey(bestTarget.getFiducialId())) {
+ 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.);
+ }
+
+ return Pair.of(
+ aprilTags
+ .get(bestTarget.getFiducialId())
+ .transformBy(bestTarget.getBestCameraToTarget().inverse())
+ .transformBy(cameras.get(lowestAI).getSecond().inverse()),
+ cameras.get(lowestAI).getFirst().getLatestResult().getLatencyMillis());
+ }
+
+ private Pair closestToCameraHeightStrategy() {
+ double smallestHeightDifference = 10e9;
+ double mili = 0;
+ Pose3d pose = lastPose;
+
+ for (int i = 0; i < cameras.size(); i++) {
+ Pair p = cameras.get(i);
+ List targets = p.getFirst().getLatestResult().targets;
+ for (int j = 0; j < targets.size(); j++) {
+ PhotonTrackedTarget target = targets.get(j);
+ // If the map doesn't contain the ID fail
+ if (!aprilTags.containsKey(target.getFiducialId())) {
+ if (!reportedErrors.contains(target.getFiducialId())) {
+ DriverStation.reportWarning(
+ "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ + target.getFiducialId(),
+ false);
+ reportedErrors.add(target.getFiducialId());
+ }
+ continue;
+ }
+ Pose3d targetPose = aprilTags.get(target.getFiducialId());
+ double alternativeDifference =
+ Math.abs(
+ p.getSecond().getZ()
+ - targetPose.transformBy(target.getAlternateCameraToTarget().inverse()).getZ());
+ double bestDifference =
+ Math.abs(
+ p.getSecond().getZ()
+ - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
+ if (alternativeDifference < smallestHeightDifference) {
+ smallestHeightDifference = alternativeDifference;
+ pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
+ mili = p.getFirst().getLatestResult().getLatencyMillis();
+ }
+ if (bestDifference < smallestHeightDifference) {
+ smallestHeightDifference = bestDifference;
+ pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
+ mili = p.getFirst().getLatestResult().getLatencyMillis();
+ }
+ }
+ }
+ return Pair.of(pose, mili);
+ }
+
+ private Pair closestToReferencePoseStrategy() {
+ if (referencePose == null) {
+ DriverStation.reportError(
+ "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
+ false);
+ return Pair.of(lastPose, 0.);
+ }
+ double smallestDifference = 10e9;
+ double mili = 0;
+ Pose3d pose = lastPose;
+ for (int i = 0; i < cameras.size(); i++) {
+ Pair p = cameras.get(i);
+ List targets = p.getFirst().getLatestResult().targets;
+ for (int j = 0; j < targets.size(); j++) {
+ PhotonTrackedTarget target = targets.get(j);
+ // If the map doesn't contain the ID fail
+ if (!aprilTags.containsKey(target.getFiducialId())) {
+ if (!reportedErrors.contains(target.getFiducialId())) {
+ DriverStation.reportWarning(
+ "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ + target.getFiducialId(),
+ false);
+ reportedErrors.add(target.getFiducialId());
+ }
+ continue;
+ }
+ Pose3d targetPose = aprilTags.get(target.getFiducialId());
+ double alternativeDifference =
+ Math.abs(
+ calculateDifference(
+ referencePose,
+ targetPose.transformBy(target.getAlternateCameraToTarget().inverse())));
+ double bestDifference =
+ Math.abs(
+ calculateDifference(
+ referencePose,
+ targetPose.transformBy(target.getBestCameraToTarget().inverse())));
+ if (alternativeDifference < smallestDifference) {
+ smallestDifference = alternativeDifference;
+ pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
+ mili = p.getFirst().getLatestResult().getLatencyMillis();
+ }
+ if (bestDifference < smallestDifference) {
+ smallestDifference = bestDifference;
+ pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
+ mili = p.getFirst().getLatestResult().getLatencyMillis();
+ }
+ }
+ }
+ return Pair.of(pose, mili);
+ }
+
+ /** Return the average of the best target poses using ambiguity as weight */
+ private Pair averageBestTargetsStrategy() {
+ // Pair of Double, Double = Ambiguity, Mili
+ List>> tempPoses = new ArrayList<>();
+ double totalAmbiguity = 0;
+ for (int i = 0; i < cameras.size(); i++) {
+ Pair p = cameras.get(i);
+ List targets = p.getFirst().getLatestResult().targets;
+ for (int j = 0; j < targets.size(); j++) {
+ PhotonTrackedTarget target = targets.get(j);
+ // If the map doesn't contain the ID fail
+ if (!aprilTags.containsKey(target.getFiducialId())) {
+ if (!reportedErrors.contains(target.getFiducialId())) {
+ DriverStation.reportWarning(
+ "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+ + target.getFiducialId(),
+ false);
+ reportedErrors.add(target.getFiducialId());
+ }
+ continue;
+ }
+ Pose3d targetPose = aprilTags.get(target.getFiducialId());
+ try {
+ totalAmbiguity += 1. / target.getPoseAmbiguity();
+ } catch (ArithmeticException e) {
+ // A total ambiguity of zero exists, using that pose instead!",
+ return Pair.of(
+ targetPose.transformBy(target.getBestCameraToTarget().inverse()),
+ p.getFirst().getLatestResult().getLatencyMillis());
+ }
+ tempPoses.add(
+ Pair.of(
+ targetPose.transformBy(target.getBestCameraToTarget().inverse()),
+ Pair.of(
+ target.getPoseAmbiguity(), p.getFirst().getLatestResult().getLatencyMillis())));
+ }
+ }
+
+ Translation3d transform = new Translation3d();
+ Rotation3d rotation = new Rotation3d();
+ double latency = 0;
+
+ for (Pair> 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);
+ }
+
+ /**
+ * Difference is defined as the vector magnitude between the two poses
+ *
+ * @return The absolute "difference" (>=0) between two Pose3ds.
+ */
+ private double calculateDifference(Pose3d x, Pose3d y) {
+ return x.getTranslation().getDistance(y.getTranslation());
+ }
+
+ /** @param aprilTags the aprilTags to set */
+ public void setAprilTags(Map aprilTags) {
+ this.aprilTags = aprilTags;
+ }
+
+ /** @return the aprilTags */
+ public Map getAprilTags() {
+ return aprilTags;
+ }
+
+ /** @return the strategy */
+ public PoseStrategy getStrategy() {
+ return strategy;
+ }
+
+ /** @param strategy the strategy to set */
+ public void setStrategy(PoseStrategy strategy) {
+ this.strategy = strategy;
+ }
+
+ /** @return the referencePose */
+ public Pose3d getReferencePose() {
+ return referencePose;
+ }
+
+ /**
+ * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
+ *
+ * @param referencePose the referencePose to set
+ */
+ public void setReferencePose(Pose3d referencePose) {
+ this.referencePose = referencePose;
+ }
+
+ /**
+ * UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
+ *
+ * @param lastPose the lastPose to set
+ */
+ public void setLastPose(Pose3d lastPose) {
+ this.lastPose = lastPose;
+ }
+}
diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
index 02186c91c..f885b9b2a 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
@@ -61,6 +61,7 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
cameraName) {}
PhotonPipelineResult PhotonCamera::GetLatestResult() {
+ if (test) return testResult;
// Prints warning if not connected
VerifyVersion();
diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
index 9f37443b7..65c78b52c 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
@@ -34,7 +34,8 @@ namespace photonlib {
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int id,
- const frc::Transform3d& pose,
+ const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
+ double ambiguity,
const wpi::SmallVector, 4> corners)
: yaw(yaw),
pitch(pitch),
@@ -42,6 +43,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
skew(skew),
fiducialId(id),
bestCameraToTarget(pose),
+ altCameraToTarget(alternatePose),
+ poseAmbiguity(ambiguity),
corners(corners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
new file mode 100644
index 000000000..73d504c3e
--- /dev/null
+++ b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
@@ -0,0 +1,265 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "photonlib/RobotPoseEstimator.h"
+
+#include
+#include
+#include