Expose pose ambiguity

photon-client/src/store/index.js

@@ -94,7 +94,8 @@ export default new Vuex.Store({
                     blur: 0.0,
                     threads: 1,
                     debug: false,
-                    refineEdges: true
+                    refineEdges: true,
+                    numIterations: 1,
                 }
             }
         ],

photon-client/src/views/PipelineViews/AprilTagTab.vue

@@ -12,40 +12,55 @@
     <CVslider
       v-model="decimate"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Decimate"
       min="0"
       max="3"
       step=".5"
+      tooltip="Increases FPS at the expense of range by reducing image resolution initially"
       @input="handlePipelineData('decimate')"
     />
     <CVslider
       v-model="blur"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Blur"
       min="0"
       max="5"
       step=".01"
+      tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
       @input="handlePipelineData('blur')"
     />
     <CVslider
       v-model="threads"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Threads"
       min="1"
       max="8"
       step="1"
+      tooltip="Number of threads spawned by the AprilTag detector"
       @input="handlePipelineData('threads')"
     />
     <CVswitch
       v-model="refineEdges"
       class="pt-2"
-      slider-cols="12"
+      slider-cols="8"
       name="Refine Edges"
+      tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
       @input="handlePipelineData('refineEdges')"
     />
+    <CVslider
+      v-model="numIterations"
+      class="pt-2 pb-4"
+      slider-cols="8"
+      name="Pose Estimation Iterations"
+      min="0"
+      max="500"
+      step="1"
+      tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
+      @input="handlePipelineData('numIterations')"
+    />
   </div>
 </template>
 
@@ -82,6 +97,14 @@
                 this.$store.commit("mutatePipeline", {"decimate": val});
             }
           },
+          numIterations: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.numIterations
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"numIterations": val});
+            }
+          },
           blur: {
             get() {
                 return this.$store.getters.currentPipelineSettings.blur

photon-client/src/views/PipelineViews/TargetsTab.vue

@@ -46,6 +46,11 @@
                   Z Angle, °
                 </th>
               </template>
+              <template v-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
+                <th class="text-center" >
+                  Ambiguity
+                </th>
+              </template>
             </tr>
           </thead>
           <tbody>
@@ -73,6 +78,11 @@
                 <td>{{ parseFloat(value.pose.y).toFixed(2) }}&nbsp;m</td>
                 <td>{{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}&deg;</td>
               </template>
+              <template v-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
+              <td>
+                {{ parseFloat(value.ambiguity).toFixed(2) }}
+              </td>
+              </template>
             </tr>
           </tbody>
         </template>

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

@@ -233,6 +233,7 @@ public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTar
                             t.getSkew(),
                             t.getFiducialId(),
                             t.getCameraToTarget3d(),
+                            t.getPoseAmbiguity(),
                             cornerList));
         }
         return ret;

photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java

@@ -135,6 +135,21 @@ public DetectionResult(
                         new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)));
     }
 
+    /**
+     * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
+     * ambiguous.
+     */
+    public double getPoseAmbiguity() {
+        var min = Math.min(error1, error2);
+        var max = Math.max(error1, error2);
+
+        if (max > 0) {
+            return min / max;
+        } else {
+            return -1;
+        }
+    }
+
     @Override
     public String toString() {
         return "DetectionResult [centerX="

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

@@ -51,6 +51,7 @@ public class TrackedTarget implements Releasable {
     private CVShape m_shape;
 
     private int m_fiducialId = -1;
+    private double m_poseAmbiguity = -1;
 
     private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
 
@@ -117,6 +118,8 @@ public TrackedTarget(DetectionResult result, TargetCalculationParameters params)
         var axis = bestPose.getRotation().getAxis().times(angle);
         rvec.put(0, 0, axis.getData());
         setCameraRelativeRvec(rvec);
+
+        m_poseAmbiguity = result.getPoseAmbiguity();
     }
 
     public void setFiducialId(int id) {
@@ -127,6 +130,14 @@ public int getFiducialId() {
         return m_fiducialId;
     }
 
+    public void setPoseAmbiguity(double ambiguity) {
+        m_poseAmbiguity = ambiguity;
+    }
+
+    public double getPoseAmbiguity() {
+        return m_poseAmbiguity;
+    }
+
     /**
      * Set the approximate bouding polygon.
      *
@@ -260,6 +271,7 @@ public HashMap<String, Object> toHashMap() {
         ret.put("yaw", getYaw());
         ret.put("skew", getSkew());
         ret.put("area", getArea());
+        ret.put("ambiguity", getPoseAmbiguity());
         if (getCameraToTarget3d() != null) {
             ret.put("pose", transformToMap(getCameraToTarget3d()));
         }

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(),
+                                        0.25,
                                         List.of(
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0),
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0))));

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

@@ -62,7 +62,8 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
          << target.cameraToTarget.Rotation().GetQuaternion().W()
          << target.cameraToTarget.Rotation().GetQuaternion().X()
          << target.cameraToTarget.Rotation().GetQuaternion().Y()
-         << target.cameraToTarget.Rotation().GetQuaternion().Z();
+         << target.cameraToTarget.Rotation().GetQuaternion().Z()
+         << target.poseAmbiguity;
 
   for (int i = 0; i < 4; i++) {
     packet << target.corners[i].first << target.corners[i].second;
@@ -86,6 +87,8 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
 
   target.cameraToTarget = frc::Transform3d(translation, rotation);
 
+  packet >> target.poseAmbiguity;
+
   target.corners.clear();
   for (int i = 0; i < 4; i++) {
     double first = 0;

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

@@ -90,6 +90,12 @@ class PhotonTrackedTarget {
     return corners;
   }
 
+  /**
+   * Get the ratio of pose reprojection errors, called ambiguity. Numbers above
+   * 0.2 are likely to be ambiguous. -1 if invalid.
+   */
+  double GetPoseAmbiguity() const { return poseAmbiguity; }
+
   /**
    * Returns the pose of the target relative to the robot.
    * @return The pose of the target relative to the robot.
@@ -109,6 +115,7 @@ class PhotonTrackedTarget {
   double skew = 0;
   int fiducialId;
   frc::Transform3d cameraToTarget;
+  double poseAmbiguity;
   wpi::SmallVector<std::pair<double, double>, 4> corners;
 };
 }  // namespace photonlib

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

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

photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java

@@ -27,14 +27,15 @@
 import org.photonvision.common.dataflow.structures.Packet;
 
 public class PhotonTrackedTarget {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4);
+    public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1);
 
     private double yaw;
     private double pitch;
     private double area;
     private double skew;
     private int fiducialId;
     private Transform3d cameraToTarget = new Transform3d();
+    private double poseAmbiguity;
     private List<TargetCorner> targetCorners;
 
     public PhotonTrackedTarget() {}
@@ -47,6 +48,7 @@ public PhotonTrackedTarget(
             double skew,
             int id,
             Transform3d pose,
+            double ambiguity,
             List<TargetCorner> corners) {
         assert corners.size() == 4;
         this.yaw = yaw;
@@ -56,6 +58,7 @@ public PhotonTrackedTarget(
         this.fiducialId = id;
         this.cameraToTarget = pose;
         this.targetCorners = corners;
+        this.poseAmbiguity = ambiguity;
     }
 
     public double getYaw() {
@@ -79,6 +82,14 @@ public int getFiducialId() {
         return fiducialId;
     }
 
+    /**
+     * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
+     * ambiguous. -1 if invalid.
+     */
+    public double getPoseAmbiguity() {
+        return poseAmbiguity;
+    }
+
     /**
      * Return a list of the 4 corners in image space (origin top left, x left, y down), in no
      * particular order, of the minimum area bounding rectangle of this target
@@ -134,6 +145,8 @@ public Packet createFromPacket(Packet packet) {
         y = packet.decodeDouble();
         z = packet.decodeDouble();
         var rotation = new Rotation3d(new Quaternion(w, x, y, z));
+        this.cameraToTarget = new Transform3d(translation, rotation);
+        this.poseAmbiguity = packet.decodeDouble();
 
         this.targetCorners = new ArrayList<>(4);
         for (int i = 0; i < 4; i++) {
@@ -142,8 +155,6 @@ public Packet createFromPacket(Packet packet) {
             targetCorners.add(new TargetCorner(cx, cy));
         }
 
-        this.cameraToTarget = new Transform3d(translation, rotation);
-
         return packet;
     }
 
@@ -166,6 +177,7 @@ public Packet populatePacket(Packet packet) {
         packet.encode(cameraToTarget.getRotation().getQuaternion().getX());
         packet.encode(cameraToTarget.getRotation().getQuaternion().getY());
         packet.encode(cameraToTarget.getRotation().getQuaternion().getZ());
+        packet.encode(poseAmbiguity);
 
         for (int i = 0; i < 4; i++) {
             packet.encode(targetCorners.get(i).x);