Multi-tag pnp in robot code (#787)

---------

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Joseph Farkas <16584585+MrRedness@users.noreply.github.com>

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

@@ -28,6 +28,7 @@
 #include <string>
 
 #include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/IntegerTopic.h>
 #include <networktables/MultiSubscriber.h>
@@ -40,6 +41,10 @@
 
 #include "photonlib/PhotonPipelineResult.h"
 
+namespace cv {
+class Mat;
+}  // namespace cv
+
 namespace photonlib {
 
 enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
@@ -144,6 +149,9 @@ class PhotonCamera {
    */
   const std::string_view GetCameraName() const;
 
+  std::optional<cv::Mat> GetCameraMatrix();
+  std::optional<cv::Mat> GetDistCoeffs();
+
   /**
    * Returns whether the latest target result has targets.
    * This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should
@@ -178,6 +186,9 @@ class PhotonCamera {
   nt::IntegerSubscriber ledModeSub;
   nt::StringSubscriber versionEntry;
 
+  nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
+  nt::DoubleArraySubscriber cameraDistortionSubscriber;
+
   nt::BooleanSubscriber driverModeSubscriber;
   nt::BooleanPublisher driverModePublisher;
   nt::IntegerSubscriber ledModeSubscriber;

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

@@ -33,13 +33,18 @@
 #include "photonlib/PhotonCamera.h"
 #include "photonlib/PhotonPipelineResult.h"
 
+namespace cv {
+class Mat;
+}  // namespace cv
+
 namespace photonlib {
-enum PoseStrategy : int {
-  LOWEST_AMBIGUITY,
+enum PoseStrategy {
+  LOWEST_AMBIGUITY = 0,
   CLOSEST_TO_CAMERA_HEIGHT,
   CLOSEST_TO_REFERENCE_POSE,
   CLOSEST_TO_LAST_POSE,
-  AVERAGE_BEST_TARGETS
+  AVERAGE_BEST_TARGETS,
+  MULTI_TAG_PNP
 };
 
 struct EstimatedRobotPose {
@@ -49,8 +54,14 @@ struct EstimatedRobotPose {
    * the same timebase as the RoboRIO FPGA Timestamp */
   units::second_t timestamp;
 
-  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_)
-      : estimatedPose(pose_), timestamp(time_) {}
+  /** A list of the targets used to compute this pose */
+  wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
+
+  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
+                     std::span<const PhotonTrackedTarget> targets)
+      : estimatedPose(pose_),
+        timestamp(time_),
+        targetsUsed(targets.data(), targets.data() + targets.size()) {}
 };
 
 /**
@@ -100,6 +111,14 @@ class PhotonPoseEstimator {
    */
   inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
 
+  /**
+   * Set the Position Estimation Strategy used in multi-tag mode when
+   * only one tag can be seen. Must NOT be MULTI_TAG_PNP
+   *
+   * @param strategy the strategy to set
+   */
+  void SetMultiTagFallbackStrategy(PoseStrategy strategy);
+
   /**
    * Return the reference position that is being used by the estimator.
    *
@@ -159,6 +178,7 @@ class PhotonPoseEstimator {
  private:
   frc::AprilTagFieldLayout aprilTags;
   PoseStrategy strategy;
+  PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
 
   PhotonCamera camera;
   frc::Transform3d m_robotToCamera;
@@ -166,6 +186,9 @@ class PhotonPoseEstimator {
   frc::Pose3d lastPose;
   frc::Pose3d referencePose;
 
+  std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
+                                           PoseStrategy strategy);
+
   /**
    * Return the estimated position of the robot with the lowest position
    * ambiguity from a List of pipeline results.
@@ -199,12 +222,21 @@ class PhotonPoseEstimator {
   std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
       PhotonPipelineResult result);
 
+  /**
+   * Return the pose calculation using all targets in view in the same PNP()
+   calculation
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
+      PhotonPipelineResult result);
+
   /**
    * Return the average of the best target poses using ambiguity as weight.
 
    * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this
-   *     estimation.
+   timestamp of this estimation.
    */
   std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
       PhotonPipelineResult result);