Run multitag on coprocessor (#816)

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<int16_t>(target.fiducialIdsUsed[i]);
+    } else {
+      packet << static_cast<int16_t>(-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

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<int8_t>(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();

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<EstimatedRobotPose> 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<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
       fiducialPose.value()
           .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
           .TransformBy(m_robotToCamera.Inverse()),
-      result.GetTimestamp(), result.GetTargets()};
+      result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
 }
 
 std::optional<EstimatedRobotPose>
@@ -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<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
@@ -351,7 +357,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
                 Rotation3d(rv));
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
+    PhotonPipelineResult result, std::optional<cv::Mat> camMat,
+    std::optional<cv::Mat> 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<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
     PhotonPipelineResult result, std::optional<cv::Mat> camMat,
     std::optional<cv::Mat> distCoeffs) {
   using namespace frc;
@@ -404,7 +427,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
 
   return photonlib::EstimatedRobotPose(
       pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
-      result.GetTargets());
+      result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
 }
 
 std::optional<EstimatedRobotPose>
@@ -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

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

@@ -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 <iostream>
-#include <limits>
-#include <map>
-#include <span>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include <frc/Errors.h>
-#include <frc/apriltag/AprilTagFieldLayout.h>
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Rotation3d.h>
-#include <frc/geometry/Transform3d.h>
-#include <units/time.h>
-
-#include "photonlib/PhotonCamera.h"
-#include "photonlib/PhotonPipelineResult.h"
-#include "photonlib/PhotonTrackedTarget.h"
-
-namespace photonlib {
-RobotPoseEstimator::RobotPoseEstimator(
-    std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
-    std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
-        cams)
-    : aprilTags(tags),
-      strategy(strat),
-      cameras(std::move(cams)),
-      lastPose(frc::Pose3d()),
-      referencePose(frc::Pose3d()) {}
-
-std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
-  if (cameras.empty()) {
-    return std::make_pair(lastPose, units::second_t(0));
-  }
-
-  std::pair<frc::Pose3d, units::second_t> pair;
-  switch (strategy) {
-    case LOWEST_AMBIGUITY:
-      pair = LowestAmbiguityStrategy();
-      lastPose = pair.first;
-      return pair;
-    case CLOSEST_TO_CAMERA_HEIGHT:
-      pair = ClosestToCameraHeightStrategy();
-      lastPose = pair.first;
-      return pair;
-    case CLOSEST_TO_REFERENCE_POSE:
-      pair = ClosestToReferencePoseStrategy();
-      lastPose = pair.first;
-      return pair;
-    case CLOSEST_TO_LAST_POSE:
-      SetReferencePose(lastPose);
-      pair = ClosestToReferencePoseStrategy();
-      lastPose = pair.first;
-      return pair;
-    case AVERAGE_BEST_TARGETS:
-      pair = AverageBestTargetsStrategy();
-      lastPose = pair.first;
-      return pair;
-    default:
-      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
-                      "");
-  }
-
-  return std::make_pair(lastPose, units::second_t(0));
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::LowestAmbiguityStrategy() {
-  int lowestAI = -1;
-  int lowestAJ = -1;
-  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
-        lowestAI = i;
-        lowestAJ = j;
-        lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
-      }
-    }
-  }
-
-  if (lowestAI == -1 || lowestAJ == -1) {
-    return std::make_pair(lastPose, units::second_t(0));
-  }
-
-  PhotonTrackedTarget bestTarget =
-      cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
-
-  std::optional<frc::Pose3d> fiducialPose =
-      aprilTags->GetTagPose(bestTarget.GetFiducialId());
-  if (!fiducialPose) {
-    FRC_ReportError(frc::warn::Warning,
-                    "Tried to get pose of unknown April Tag: {}",
-                    bestTarget.GetFiducialId());
-    return std::make_pair(lastPose, units::second_t(0));
-  }
-
-  return std::make_pair(
-      fiducialPose.value()
-          .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
-          .TransformBy(cameras[lowestAI].second.Inverse()),
-      cameras[lowestAI].first->GetLatestResult().GetTimestamp());
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::ClosestToCameraHeightStrategy() {
-  units::meter_t smallestHeightDifference =
-      units::meter_t(std::numeric_limits<double>::infinity());
-  units::second_t stateTimestamp = units::second_t(0);
-  frc::Pose3d pose = lastPose;
-
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      PhotonTrackedTarget target = targets[j];
-      std::optional<frc::Pose3d> fiducialPose =
-          aprilTags->GetTagPose(target.GetFiducialId());
-      if (!fiducialPose) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Tried to get pose of unknown April Tag: {}",
-                        target.GetFiducialId());
-        continue;
-      }
-      frc::Pose3d targetPose = fiducialPose.value();
-      units::meter_t alternativeDifference = units::math::abs(
-          p.second.Z() -
-          targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
-              .Z());
-      units::meter_t bestDifference = units::math::abs(
-          p.second.Z() -
-          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
-      if (alternativeDifference < smallestHeightDifference) {
-        smallestHeightDifference = alternativeDifference;
-        pose = targetPose.TransformBy(
-            target.GetAlternateCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-      if (bestDifference < smallestHeightDifference) {
-        smallestHeightDifference = bestDifference;
-        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-    }
-  }
-  return std::make_pair(pose, stateTimestamp);
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::ClosestToReferencePoseStrategy() {
-  units::meter_t smallestDifference =
-      units::meter_t(std::numeric_limits<double>::infinity());
-  units::second_t stateTimestamp = units::second_t(0);
-  frc::Pose3d pose = lastPose;
-
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      PhotonTrackedTarget target = targets[j];
-      std::optional<frc::Pose3d> fiducialPose =
-          aprilTags->GetTagPose(target.GetFiducialId());
-      if (!fiducialPose) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Tried to get pose of unknown April Tag: {}",
-                        target.GetFiducialId());
-        continue;
-      }
-      frc::Pose3d targetPose = fiducialPose.value();
-      units::meter_t alternativeDifference =
-          units::math::abs(referencePose.Translation().Distance(
-              targetPose
-                  .TransformBy(target.GetAlternateCameraToTarget().Inverse())
-                  .Translation()));
-      units::meter_t bestDifference =
-          units::math::abs(referencePose.Translation().Distance(
-              targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
-                  .Translation()));
-      if (alternativeDifference < smallestDifference) {
-        smallestDifference = alternativeDifference;
-        pose = targetPose.TransformBy(
-            target.GetAlternateCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-
-      if (bestDifference < smallestDifference) {
-        smallestDifference = bestDifference;
-        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
-      }
-    }
-  }
-
-  return std::make_pair(pose, stateTimestamp);
-}
-
-std::pair<frc::Pose3d, units::second_t>
-RobotPoseEstimator::AverageBestTargetsStrategy() {
-  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
-      tempPoses;
-  double totalAmbiguity = 0;
-  units::second_t timstampSum = units::second_t(0);
-
-  for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
-    std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
-    std::span<const PhotonTrackedTarget> targets =
-        p.first->GetLatestResult().GetTargets();
-    timstampSum += p.first->GetLatestResult().GetTimestamp();
-    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-      PhotonTrackedTarget target = targets[j];
-      std::optional<frc::Pose3d> fiducialPose =
-          aprilTags->GetTagPose(target.GetFiducialId());
-      if (!fiducialPose) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Tried to get pose of unknown April Tag: {}",
-                        target.GetFiducialId());
-        continue;
-      }
-
-      frc::Pose3d targetPose = fiducialPose.value();
-      if (target.GetPoseAmbiguity() == 0) {
-        FRC_ReportError(frc::warn::Warning,
-                        "Pose ambiguity of zero exists, using that instead!",
-                        "");
-        return std::make_pair(
-            targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
-            p.first->GetLatestResult().GetLatency() / 1000.);
-      }
-      totalAmbiguity += 1. / target.GetPoseAmbiguity();
-
-      tempPoses.push_back(std::make_pair(
-          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
-          std::make_pair(target.GetPoseAmbiguity(),
-                         p.first->GetLatestResult().GetTimestamp())));
-    }
-  }
-
-  frc::Translation3d transform = frc::Translation3d();
-  frc::Rotation3d rotation = frc::Rotation3d();
-
-  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
-       tempPoses) {
-    double weight = (1. / pair.second.first) / totalAmbiguity;
-    transform = transform + pair.first.Translation() * weight;
-    rotation = rotation + pair.first.Rotation() * weight;
-  }
-
-  return std::make_pair(frc::Pose3d(transform, rotation),
-                        timstampSum / cameras.size());
-}
-}  // namespace photonlib

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

@@ -0,0 +1,61 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/geometry/Transform3d.h>
+#include <wpi/SmallVector.h>
+
+#include "photonlib/Packet.h"
+
+namespace photonlib {
+
+class PNPResults {
+ public:
+  // This could be wrapped in an std::optional, but chose to do it this way to
+  // mirror Java
+  bool isValid;
+
+  frc::Transform3d best;
+  double bestReprojectionErr;
+
+  frc::Transform3d alt;
+  double altReprojectionErr;
+
+  double ambiguity;
+
+  friend Packet& operator<<(Packet& packet, const PNPResults& result);
+  friend Packet& operator>>(Packet& packet, PNPResults& result);
+};
+
+class MultiTargetPnpResult {
+ public:
+  PNPResults result;
+  wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
+
+  friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result);
+  friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result);
+};
+
+}  // namespace photonlib

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

@@ -31,6 +31,7 @@
 #include <units/time.h>
 #include <wpi/SmallVector.h>
 
+#include "photonlib/MultiTargetPNPResult.h"
 #include "photonlib/Packet.h"
 #include "photonlib/PhotonTrackedTarget.h"
 
@@ -87,6 +88,13 @@ class PhotonPipelineResult {
    */
   units::second_t GetTimestamp() const { return timestamp; }
 
+  /**
+   * Return the latest mulit-target result, as calculated on your coprocessor.
+   * Be sure to check getMultiTagResult().estimatedPose.isValid before using the
+   * pose estimate!
+   */
+  const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; }
+
   /**
    * Sets the timestamp in seconds
    * @param timestamp The timestamp in seconds
@@ -119,6 +127,7 @@ class PhotonPipelineResult {
   units::second_t latency = 0_s;
   units::second_t timestamp = -1_s;
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
+  MultiTargetPnpResult m_pnpResults;
   inline static bool HAS_WARNED = false;
 };
 }  // namespace photonlib

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

@@ -44,7 +44,8 @@ enum PoseStrategy {
   CLOSEST_TO_REFERENCE_POSE,
   CLOSEST_TO_LAST_POSE,
   AVERAGE_BEST_TARGETS,
-  MULTI_TAG_PNP
+  MULTI_TAG_PNP_ON_COPROCESSOR,
+  MULTI_TAG_PNP_ON_RIO,
 };
 
 struct EstimatedRobotPose {
@@ -57,11 +58,16 @@ struct EstimatedRobotPose {
   /** A list of the targets used to compute this pose */
   wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
 
+  /** The strategy actually used to produce this pose */
+  PoseStrategy strategy;
+
   EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
-                     std::span<const PhotonTrackedTarget> targets)
+                     std::span<const PhotonTrackedTarget> targets,
+                     PoseStrategy strategy_)
       : estimatedPose(pose_),
         timestamp(time_),
-        targetsUsed(targets.data(), targets.data() + targets.size()) {}
+        targetsUsed(targets.data(), targets.data() + targets.size()),
+        strategy(strategy_) {}
 };
 
 /**
@@ -260,14 +266,23 @@ class PhotonPoseEstimator {
   std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
       PhotonPipelineResult result);
 
+  /**
+   * Return the pose calculated by combining all tags into one on coprocessor
+   *
+   * @return the estimated position of the robot in the FCS
+   */
+  std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
+      PhotonPipelineResult result, std::optional<cv::Mat> camMat,
+      std::optional<cv::Mat> distCoeffs);
+
   /**
    * 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(
+  std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
       PhotonPipelineResult result, std::optional<cv::Mat> camMat,
       std::optional<cv::Mat> distCoeffs);
 

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

@@ -1,187 +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.
- */
-
-#pragma once
-
-#include <map>
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Transform3d.h>
-
-#include "photonlib/PhotonCamera.h"
-
-namespace frc {
-class AprilTagFieldLayout;
-}  // namespace frc
-
-namespace photonlib {
-enum PoseStrategy : int {
-  LOWEST_AMBIGUITY,
-  CLOSEST_TO_CAMERA_HEIGHT,
-  CLOSEST_TO_REFERENCE_POSE,
-  CLOSEST_TO_LAST_POSE,
-  AVERAGE_BEST_TARGETS
-};
-
-/**
- * The RobotPoseEstimator class filters or combines readings from all the
- * fiducials visible at a given timestamp on the field to produce a single robot
- * in field pose, using the strategy set below. Example usage can be found in
- * our apriltagExample example project.
- */
-class RobotPoseEstimator {
- public:
-  using map_value_type =
-      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
-  using size_type = std::vector<map_value_type>::size_type;
-
-  /**
-   * Create a new RobotPoseEstimator.
-   *
-   * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
-   * <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
-   * at (1.0,2.0,3.0) </code> }
-   *
-   * @param aprilTags A AprilTagFieldLayout 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.
-   */
-  explicit RobotPoseEstimator(
-      std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
-      PoseStrategy strategy, std::vector<map_value_type> cameras);
-
-  /**
-   * Get the AprilTagFieldLayout being used by the PositionEstimator.
-   *
-   * @return the AprilTagFieldLayout
-   */
-  std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
-    return aprilTags;
-  }
-
-  /**
-   * Set the cameras to be used by the PoseEstimator.
-   *
-   * @param cameras cameras to set.
-   */
-  inline void SetCameras(
-      const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
-                                  frc::Transform3d>>& cameras) {
-    this->cameras = cameras;
-  }
-
-  /**
-   * Get the Position Estimation Strategy being used by the Position Estimator.
-   *
-   * @return the strategy
-   */
-  PoseStrategy GetPoseStrategy() const { return strategy; }
-
-  /**
-   * Set the Position Estimation Strategy used by the Position Estimator.
-   *
-   * @param strategy the strategy to set
-   */
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
-
-  /**
-   * Return the reference position that is being used by the estimator.
-   *
-   * @return the referencePose
-   */
-  frc::Pose3d GetReferencePose() const { return referencePose; }
-
-  /**
-   * Update the stored reference pose for use when using the
-   * CLOSEST_TO_REFERENCE_POSE strategy.
-   *
-   * @param referencePose the referencePose to set
-   */
-  inline void SetReferencePose(frc::Pose3d referencePose) {
-    this->referencePose = referencePose;
-  }
-
-  /**
-   * Update the stored last pose. Useful for setting the initial estimate when
-   * using the CLOSEST_TO_LAST_POSE strategy.
-   *
-   * @param lastPose the lastPose to set
-   */
-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
-
-  std::pair<frc::Pose3d, units::second_t> Update();
-
- private:
-  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
-  PoseStrategy strategy;
-  std::vector<map_value_type> cameras;
-  frc::Pose3d lastPose;
-  frc::Pose3d referencePose;
-
-  /**
-   * Return the estimated position of the robot with the lowest position
-   * ambiguity from a List of pipeline results.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta height difference between the estimated and actual height of the
-   * camera.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta in the vector magnitude between it and the reference pose.
-   *
-   * @param referencePose reference pose to check vector magnitude difference
-   * against.
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
-
-  /**
-   * 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.
-   */
-  std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
-};
-
-}  // namespace photonlib