Expose both pose solutions

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -189,7 +189,7 @@ public void accept(CVPipelineResult result) {
             targetAreaEntry.forceSetDouble(bestTarget.getArea());
             targetSkewEntry.forceSetDouble(bestTarget.getSkew());
 
-            var pose = bestTarget.getCameraToTarget3d();
+            var pose = bestTarget.getBestCameraToTarget3d();
             targetPoseEntry.forceSetDoubleArray(
                     new double[] {
                         pose.getTranslation().getX(),
@@ -232,7 +232,8 @@ public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTar
                             t.getArea(),
                             t.getSkew(),
                             t.getFiducialId(),
-                            t.getCameraToTarget3d(),
+                            t.getBestCameraToTarget3d(),
+                            t.getAltCameraToTarget3d(),
                             t.getPoseAmbiguity(),
                             cornerList));
         }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java

@@ -100,8 +100,9 @@ private void calculateTargetPose(TrackedTarget target) {
                         Core.norm(rVec));
 
         Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation));
-        target.setCameraToTarget3d(
+        target.setBestCameraToTarget3d(
                 new Transform3d(targetPose.getTranslation(), targetPose.getRotation()));
+        target.setAltCameraToTarget3d(new Transform3d());
     }
 
     Mat rotationMatrix = new Mat();

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -143,9 +143,13 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
                             new TargetCalculationParameters(
                                     false, null, null, null, null, frameStaticProperties));
 
-            var correctedPose = MathUtils.convertOpenCVtoPhotonPose(target.getCameraToTarget3d());
-            target.setCameraToTarget3d(
-                    new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));
+            var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d());
+            var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d());
+
+            target.setBestCameraToTarget3d(
+                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+            target.setAltCameraToTarget3d(
+                    new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
 
             targetList.add(target);
         }

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -46,7 +46,8 @@ public class TrackedTarget implements Releasable {
     private double m_area;
     private double m_skew;
 
-    private Transform3d m_cameraToTarget3d = new Transform3d();
+    private Transform3d m_bestCameraToTarget3d = new Transform3d();
+    private Transform3d m_altCameraToTarget3d = new Transform3d();
 
     private CVShape m_shape;
 
@@ -74,15 +75,20 @@ public TrackedTarget(DetectionResult result, TargetCalculationParameters params)
                 TargetCalculations.calculateYaw(
                         result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
         var bestPose = new Transform3d();
+        var altPose = new Transform3d();
         if (result.getError1() <= result.getError2()) {
             bestPose = result.getPoseResult1();
+            altPose = result.getPoseResult2();
         } else {
             bestPose = result.getPoseResult2();
+            altPose = result.getPoseResult1();
         }
 
         bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
+        altPose = MathUtils.convertApriltagtoOpenCV(altPose);
 
-        m_cameraToTarget3d = bestPose;
+        m_bestCameraToTarget3d = bestPose;
+        m_altCameraToTarget3d = altPose;
 
         double[] corners = result.getCorners();
         Point[] cornerPoints =
@@ -231,12 +237,20 @@ public boolean hasSubContours() {
         return !m_subContours.isEmpty();
     }
 
-    public Transform3d getCameraToTarget3d() {
-        return m_cameraToTarget3d;
+    public Transform3d getBestCameraToTarget3d() {
+        return m_bestCameraToTarget3d;
     }
 
-    public void setCameraToTarget3d(Transform3d pose) {
-        this.m_cameraToTarget3d = pose;
+    public Transform3d getAltCameraToTarget3d() {
+        return m_altCameraToTarget3d;
+    }
+
+    public void setBestCameraToTarget3d(Transform3d pose) {
+        this.m_bestCameraToTarget3d = pose;
+    }
+
+    public void setAltCameraToTarget3d(Transform3d pose) {
+        this.m_altCameraToTarget3d = pose;
     }
 
     public Mat getCameraRelativeTvec() {
@@ -272,8 +286,8 @@ public HashMap<String, Object> toHashMap() {
         ret.put("skew", getSkew());
         ret.put("area", getArea());
         ret.put("ambiguity", getPoseAmbiguity());
-        if (getCameraToTarget3d() != null) {
-            ret.put("pose", transformToMap(getCameraToTarget3d()));
+        if (getBestCameraToTarget3d() != null) {
+            ret.put("pose", transformToMap(getBestCameraToTarget3d()));
         }
         ret.put("fiducialId", getFiducialId());
         return ret;

photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java

@@ -161,7 +161,7 @@ private static void printTestResults(CVPipelineResult pipelineResult) {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget3d)
+                                .map(TrackedTarget::getBestCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }

photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java

@@ -111,7 +111,7 @@ public void test2019() {
         printTestResults(pipelineResult);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05);
 
@@ -159,7 +159,7 @@ public void test2020() {
                 pipelineResult.targets);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05);
         // Z rotation should be mostly facing us
@@ -211,7 +211,7 @@ private static void printTestResults(CVPipelineResult pipelineResult) {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget3d)
+                                .map(TrackedTarget::getBestCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }

photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java

@@ -144,7 +144,7 @@ public void submitProcessedFrame(
             targetAreaEntry.setDouble(bestTarget.getArea());
             targetSkewEntry.setDouble(bestTarget.getSkew());
 
-            var transform = bestTarget.getCameraToTarget();
+            var transform = bestTarget.getBestCameraToTarget();
             double[] poseData = {
                 transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
             };

photon-lib/src/main/java/org/photonvision/SimVisionSystem.java

@@ -171,6 +171,7 @@ public void processFrame(Pose2d robotPoseMeters) {
                                         0.0,
                                         -1, // TODO fiducial ID
                                         new Transform3d(),
+                                        new Transform3d(),
                                         0.25,
                                         List.of(
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0),

photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp

@@ -41,12 +41,12 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       area(area),
       skew(skew),
       fiducialId(id),
-      cameraToTarget(pose),
+      bestCameraToTarget(pose),
       corners(corners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&
-         other.skew == skew && other.cameraToTarget == cameraToTarget &&
+         other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
          other.corners == corners;
 }
 
@@ -56,13 +56,21 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
 
 Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
   packet << target.yaw << target.pitch << target.area << target.skew
-         << target.fiducialId << target.cameraToTarget.Translation().X().value()
-         << target.cameraToTarget.Translation().Y().value()
-         << target.cameraToTarget.Translation().Z().value()
-         << target.cameraToTarget.Rotation().GetQuaternion().W()
-         << target.cameraToTarget.Rotation().GetQuaternion().X()
-         << target.cameraToTarget.Rotation().GetQuaternion().Y()
-         << target.cameraToTarget.Rotation().GetQuaternion().Z()
+         << target.fiducialId
+         << target.bestCameraToTarget.Translation().X().value()
+         << target.bestCameraToTarget.Translation().Y().value()
+         << target.bestCameraToTarget.Translation().Z().value()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().W()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().X()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().Y()
+         << target.bestCameraToTarget.Rotation().GetQuaternion().Z()
+         << target.altCameraToTarget.Translation().X().value()
+         << target.altCameraToTarget.Translation().Y().value()
+         << target.altCameraToTarget.Translation().Z().value()
+         << target.altCameraToTarget.Rotation().GetQuaternion().W()
+         << target.altCameraToTarget.Rotation().GetQuaternion().X()
+         << target.altCameraToTarget.Rotation().GetQuaternion().Y()
+         << target.altCameraToTarget.Rotation().GetQuaternion().Z()
          << target.poseAmbiguity;
 
   for (int i = 0; i < 4; i++) {
@@ -75,17 +83,28 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
 Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
   packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
       target.fiducialId;
+
+  // We use these for best and alt transforms below
   double x = 0;
   double y = 0;
   double z = 0;
   double w = 0;
+
+  // First transform is the "best" pose
   packet >> x >> y >> z;
-  const auto translation = frc::Translation3d(
+  const auto bestTranslation = frc::Translation3d(
       units::meter_t(x), units::meter_t(y), units::meter_t(z));
   packet >> w >> x >> y >> z;
-  const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation);
 
-  target.cameraToTarget = frc::Transform3d(translation, rotation);
+  // Second transform is the "alternate" pose
+  packet >> x >> y >> z;
+  const auto altTranslation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+  packet >> w >> x >> y >> z;
+  const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
+  target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation);
 
   packet >> target.poseAmbiguity;
 

photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h

@@ -97,10 +97,22 @@ class PhotonTrackedTarget {
   double GetPoseAmbiguity() const { return poseAmbiguity; }
 
   /**
-   * Returns the pose of the target relative to the robot.
+   * Get the transform that maps camera space (X = forward, Y = left, Z = up) to
+   * object/fiducial tag space (X forward, Y left, Z up) with the lowest
+   * reprojection error. The ratio between this and the alternate target's
+   * reprojection error is the ambiguity, which is between 0 and 1.
    * @return The pose of the target relative to the robot.
    */
-  frc::Transform3d GetCameraToTarget() const { return cameraToTarget; }
+  frc::Transform3d GetBestCameraToTarget() const { return bestCameraToTarget; }
+
+  /**
+   * Get the transform that maps camera space (X = forward, Y = left, Z = up) to
+   * object/fiducial tag space (X forward, Y left, Z up) with the highest
+   * reprojection error
+   */
+  frc::Transform3d GetAlternateCameraToTarget() const {
+    return altCameraToTarget;
+  }
 
   bool operator==(const PhotonTrackedTarget& other) const;
   bool operator!=(const PhotonTrackedTarget& other) const;
@@ -114,7 +126,8 @@ class PhotonTrackedTarget {
   double area = 0;
   double skew = 0;
   int fiducialId;
-  frc::Transform3d cameraToTarget;
+  frc::Transform3d bestCameraToTarget;
+  frc::Transform3d altCameraToTarget;
   double poseAmbiguity;
   wpi::SmallVector<std::pair<double, double>, 4> corners;
 };

photon-lib/src/test/java/org/photonvision/PacketTest.java

@@ -45,6 +45,7 @@ void testSimpleTrackedTarget() {
                         -5.0,
                         -1,
                         new Transform3d(new Translation3d(), new Rotation3d()),
+                        new Transform3d(new Translation3d(), new Rotation3d()),
                         0.25,
                         List.of(
                                 new TargetCorner(1, 2),
@@ -82,6 +83,7 @@ void testSimplePipelineResult() {
                                         4.0,
                                         2,
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.25,
                                         List.of(
                                                 new TargetCorner(1, 2),
@@ -95,6 +97,7 @@ void testSimplePipelineResult() {
                                         6.7,
                                         3,
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         0.25,
                                         List.of(
                                                 new TargetCorner(1, 2),

photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -69,7 +69,7 @@ public void testApriltagFacingCamera() {
         TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
 
         // these numbers are not *accurate*, but they are known and expected
-        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
@@ -97,7 +97,7 @@ private static void printTestResults(CVPipelineResult pipelineResult) {
         System.out.println(
                 "Found targets at "
                         + pipelineResult.targets.stream()
-                                .map(TrackedTarget::getCameraToTarget3d)
+                                .map(TrackedTarget::getBestCameraToTarget3d)
                                 .collect(Collectors.toList()));
     }
 }