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