> 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);
- }
-
- /**
- * 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(AprilTagFieldLayout aprilTags) {
- this.aprilTags = aprilTags;
- }
-
- /**
- * @return the aprilTags
- */
- public AprilTagFieldLayout 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 reference pose for use with CLOSEST_TO_REFERENCE_POSE
- *
- * @param referencePose the referencePose to set
- */
- public void setReferencePose(Pose2d referencePose) {
- setReferencePose(new Pose3d(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;
- }
-
- private void reportFiducialPoseError(int fiducialId) {
- if (!reportedErrors.contains(fiducialId)) {
- DriverStation.reportError(
- "[RobotPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
- reportedErrors.add(fiducialId);
- }
- }
-}
diff --git a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java b/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
deleted file mode 100644
index 89adf94bcf..0000000000
--- a/photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 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.
- */
-
-package org.photonvision.estimation;
-
-import edu.wpi.first.math.geometry.Transform3d;
-
-/**
- * The best estimated transformation from solvePnP, and possibly an alternate transformation
- * depending on the solvePNP method. If an alternate solution is present, the ambiguity value
- * represents the ratio of reprojection error in the best solution to the alternate (best /
- * alternate).
- *
- * Note that the coordinate frame of these transforms depends on the implementing solvePnP
- * method.
- */
-public class PNPResults {
- /**
- * If this result is valid. A false value indicates there was an error in estimation, and this
- * result should not be used.
- */
- public final boolean isPresent;
-
- /**
- * The best-fit transform. The coordinate frame of this transform depends on the method which gave
- * this result.
- */
- public final Transform3d best;
-
- /** Reprojection error of the best solution, in pixels */
- public final double bestReprojErr;
-
- /**
- * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
- * to the best solution.
- */
- public final Transform3d alt;
-
- /** If no alternate solution is found, this is bestReprojErr */
- public final double altReprojErr;
-
- /** If no alternate solution is found, this is 0 */
- public final double ambiguity;
-
- /** An empty (invalid) result. */
- public PNPResults() {
- this.isPresent = false;
- this.best = new Transform3d();
- this.alt = new Transform3d();
- this.ambiguity = 0;
- this.bestReprojErr = 0;
- this.altReprojErr = 0;
- }
-
- public PNPResults(Transform3d best, double bestReprojErr) {
- this(best, best, 0, bestReprojErr, bestReprojErr);
- }
-
- public PNPResults(
- Transform3d best,
- Transform3d alt,
- double ambiguity,
- double bestReprojErr,
- double altReprojErr) {
- this.isPresent = true;
- this.best = best;
- this.alt = alt;
- this.ambiguity = ambiguity;
- this.bestReprojErr = bestReprojErr;
- this.altReprojErr = altReprojErr;
- }
-}
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
index e50588c640..1c9ef2ce1e 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -50,9 +50,9 @@
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.OpenCVHelp;
-import org.photonvision.estimation.PNPResults;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
+import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
index 9f301486a6..58e3e2fcf7 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
@@ -35,6 +35,7 @@
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
+import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -140,7 +141,8 @@ public void submitProcessedFrame(
targetList.sort(sortMode.getComparator());
}
- PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
+ PhotonPipelineResult newResult =
+ new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults());
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData());
diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp
new file mode 100644
index 0000000000..91d6ffd6c3
--- /dev/null
+++ b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp
@@ -0,0 +1,113 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 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/MultiTargetPNPResult.h"
+
+namespace photonlib {
+
+Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) {
+ packet << target.result;
+
+ size_t i;
+ for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
+ if (i < target.fiducialIdsUsed.size()) {
+ packet << static_cast(target.fiducialIdsUsed[i]);
+ } else {
+ packet << static_cast(-1);
+ }
+ }
+
+ return packet;
+}
+
+Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) {
+ packet >> target.result;
+
+ target.fiducialIdsUsed.clear();
+ for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
+ int16_t id = 0;
+ packet >> id;
+
+ if (id > -1) {
+ target.fiducialIdsUsed.push_back(id);
+ }
+ }
+
+ return packet;
+}
+
+// Encode a transform3d
+Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
+ packet << transform.Translation().X().value()
+ << transform.Translation().Y().value()
+ << transform.Translation().Z().value()
+ << transform.Rotation().GetQuaternion().W()
+ << transform.Rotation().GetQuaternion().X()
+ << transform.Rotation().GetQuaternion().Y()
+ << transform.Rotation().GetQuaternion().Z();
+
+ return packet;
+}
+
+// Decode a transform3d
+Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
+ frc::Transform3d ret;
+
+ // We use these for best and alt transforms below
+ double x = 0;
+ double y = 0;
+ double z = 0;
+ double w = 0;
+
+ // decode and unitify translation
+ packet >> x >> y >> z;
+ const auto translation = frc::Translation3d(
+ units::meter_t(x), units::meter_t(y), units::meter_t(z));
+
+ // decode and add units to rotation
+ packet >> w >> x >> y >> z;
+ const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+
+ transform = frc::Transform3d(translation, rotation);
+
+ return packet;
+}
+
+Packet& operator<<(Packet& packet, PNPResults const& result) {
+ packet << result.isValid << result.best << result.alt
+ << result.bestReprojectionErr << result.altReprojectionErr
+ << result.ambiguity;
+
+ return packet;
+}
+
+Packet& operator>>(Packet& packet, PNPResults& result) {
+ packet >> result.isValid >> result.best >> result.alt >>
+ result.bestReprojectionErr >> result.altReprojectionErr >>
+ result.ambiguity;
+
+ return packet;
+}
+
+} // namespace photonlib
diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
index 9f6d0a8218..462dd25a90 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
@@ -40,7 +40,7 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
- packet << result.latency.value() * 1000
+ packet << result.latency.value() * 1000 << result.m_pnpResults
<< static_cast(result.targets.size());
// Encode the information of each target.
@@ -52,9 +52,9 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
- int8_t targetCount = 0;
double latencyMillis = 0;
- packet >> latencyMillis >> targetCount;
+ int8_t targetCount = 0;
+ packet >> latencyMillis >> result.m_pnpResults >> targetCount;
result.latency = units::second_t(latencyMillis / 1000.0);
result.targets.clear();
diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
index f25e01eba5..080243749f 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
@@ -82,7 +82,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
poseCacheTimestamp(-1_s) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
- if (strategy == MULTI_TAG_PNP) {
+ if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
+ strategy == MULTI_TAG_PNP_ON_RIO) {
FRC_ReportError(
frc::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
@@ -162,8 +163,12 @@ std::optional PhotonPoseEstimator::Update(
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
break;
- case ::photonlib::MULTI_TAG_PNP:
- ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs);
+ case MULTI_TAG_PNP_ON_COPROCESSOR:
+ ret =
+ MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs);
+ break;
+ case MULTI_TAG_PNP_ON_RIO:
+ ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -205,7 +210,7 @@ std::optional PhotonPoseEstimator::LowestAmbiguityStrategy(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
- result.GetTimestamp(), result.GetTargets()};
+ result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
}
std::optional
@@ -241,14 +246,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
- result.GetTimestamp(), result.GetTargets()};
+ result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
- result.GetTimestamp(), result.GetTargets()};
+ result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
}
}
@@ -299,7 +304,8 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
}
}
- return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
+ return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
+ CLOSEST_TO_REFERENCE_POSE};
}
std::optional> detail::CalcTagCorners(
@@ -351,7 +357,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
Rotation3d(rv));
}
-std::optional PhotonPoseEstimator::MultiTagPnpStrategy(
+std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy(
+ PhotonPipelineResult result, std::optional camMat,
+ std::optional distCoeffs) {
+ if (result.MultiTagResult().result.isValid) {
+ const auto field2camera = result.MultiTagResult().result.best;
+
+ const auto fieldToRobot =
+ frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
+ return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
+ result.GetTargets(),
+ MULTI_TAG_PNP_ON_COPROCESSOR);
+ }
+
+ return Update(result, std::nullopt, std::nullopt,
+ this->multiTagFallbackStrategy);
+}
+
+std::optional PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional camMat,
std::optional distCoeffs) {
using namespace frc;
@@ -404,7 +427,7 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy(
return photonlib::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
- result.GetTargets());
+ result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
}
std::optional
@@ -430,7 +453,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
- result.GetTimestamp(), result.GetTargets()};
+ result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
@@ -450,6 +473,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
- result.GetTimestamp(), result.GetTargets()};
+ result.GetTimestamp(), result.GetTargets(),
+ AVERAGE_BEST_TARGETS};
}
} // namespace photonlib
diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
deleted file mode 100644
index 4d25a0a482..0000000000
--- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
+++ /dev/null
@@ -1,282 +0,0 @@
-/*
- * MIT License
- *
- * Copyright (c) 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