Finish cleanup

PhotonVision · Nov 17, 2023 · 2726a3d · 2726a3d
1 parent cb1952f
commit 2726a3d
Show file tree

Hide file tree

Showing 8 changed files with 53 additions and 23 deletions.
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp
@@ -18,3 +18,10 @@
 #include "photon/targeting/MultiTargetPNPResult.h"
 
 using namespace photon;
+
+photon::MultiTargetPNPResult::MultiTargetPNPResult(PNPResult result, wpi::SmallVector<int16_t, 32> fiducialIdsUsed) : result(result), fiducialIdsUsed(fiducialIdsUsed) {}
+
+bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other)
+const {
+  return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed;
+}
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp
@@ -18,3 +18,20 @@
 #include "photon/targeting/PNPResult.h"
 
 using namespace photon;
+
+PNPResult::PNPResult(frc::Transform3d best, double bestReprojErr,
+                             frc::Transform3d alt, double altReprojErr,
+                             double ambiguity) : best(best), bestReprojErr(bestReprojErr), alt(alt), altReprojErr(altReprojErr), ambiguity(ambiguity) {
+                                this->isPresent = true;
+                             }
+
+
+
+bool PNPResult::operator==(const PNPResult& other) const {
+  return other.isPresent == isPresent 
+            && other.best == best 
+            && other.altReprojErr == bestReprojErr 
+            && other.alt == alt 
+            && other.altReprojErr == altReprojErr 
+            && other.ambiguity == ambiguity;
+}
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp
@@ -16,6 +16,7 @@
  */
 
 #include "photon/targeting/PhotonPipelineResult.h"
+#include "photon/targeting/PNPResult.h"
 
 using namespace photon;
 
@@ -24,10 +25,11 @@ PhotonPipelineResult::PhotonPipelineResult(
     : latency(latency),
       targets(targets.data(), targets.data() + targets.size()) {}
 
-bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
-  return latency == other.latency && targets == other.targets;
-}
+PhotonPipelineResult::PhotonPipelineResult(
+    units::second_t latency, std::span<const PhotonTrackedTarget> targets,
+    MultiTargetPNPResult multitagResult) : latency(latency), targets(targets.data(), targets.data() + targets.size()), multitagResult(multitagResult) {}
 
-bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
-  return !operator==(other);
+bool PhotonPipelineResult::operator==(
+    const PhotonPipelineResult& other) const {
+  return latency == other.latency && targets == other.targets && multitagResult == other.multitagResult;
 }
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp
@@ -16,14 +16,14 @@
  */
 
 #include "photon/targeting/PhotonTrackedTarget.h"
+#include "photon/targeting/TargetCorner.h"
 
 #include <iostream>
 #include <utility>
 
 #include <frc/geometry/Translation2d.h>
 #include <wpi/SmallVector.h>
 
-#include "photon/targeting/TargetCorner.h"
 
 static constexpr const uint8_t MAX_CORNERS = 8;
 
@@ -51,7 +51,3 @@ bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
          other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
          other.minAreaRectCorners == minAreaRectCorners;
 }
-
-bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
-  return !operator==(other);
-}
diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h
@@ -23,9 +23,17 @@
 #include "PNPResult.h"
 
 namespace photon {
-class MultiTargetPnpResult {
+class MultiTargetPNPResult {
  public:
+  MultiTargetPNPResult() = default;
+
+
+  MultiTargetPNPResult(PNPResult result,
+                       wpi::SmallVector<int16_t, 32> fiducialIdsUsed);
+
   PNPResult result;
   wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
+
+  bool operator==(const MultiTargetPNPResult& other) const;
 };
 }  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h
@@ -25,19 +25,21 @@ class PNPResult {
  public:
   PNPResult() = default;
 
-  PNPResult(frc::Transform3d best, frc::Transform3d alt, double ambiguity,
-            double bestReprojErr, double altReprojErr);
+  PNPResult(frc::Transform3d best, double bestReprojErr, frc::Transform3d alt,
+            double altReprojErr, double ambiguity);
 
   // This could be wrapped in an std::optional, but chose to do it this way to
   // mirror Java
-  bool isValid;
+  bool isPresent;
 
   frc::Transform3d best;
-  double bestReprojectionErr;
+  double bestReprojErr;
 
   frc::Transform3d alt;
-  double altReprojectionErr;
+  double altReprojErr;
 
   double ambiguity;
+
+  bool operator==(const PNPResult& other) const;
 };
 }  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h
@@ -54,7 +54,7 @@ class PhotonPipelineResult {
    */
   PhotonPipelineResult(units::second_t latency,
                        std::span<const PhotonTrackedTarget> targets,
-                       MultiTargetPnpResult multitagResult);
+                       MultiTargetPNPResult multitagResult);
 
   /**
    * Returns the best target in this pipeline result. If there are no targets,
@@ -92,10 +92,10 @@ class PhotonPipelineResult {
 
   /**
    * Return the latest mulit-target result, as calculated on your coprocessor.
-   * Be sure to check getMultiTagResult().estimatedPose.isValid before using the
-   * pose estimate!
+   * Be sure to check getMultiTagResult().estimatedPose.isPresent before using
+   * the pose estimate!
    */
-  const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResult; }
+  const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; }
 
   /**
    * Sets the timestamp in seconds
@@ -120,13 +120,12 @@ class PhotonPipelineResult {
   }
 
   bool operator==(const PhotonPipelineResult& other) const;
-  bool operator!=(const PhotonPipelineResult& other) const;
 
  private:
   units::second_t latency = 0_s;
   units::second_t timestamp = -1_s;
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
-  MultiTargetPnpResult m_pnpResult;
+  MultiTargetPNPResult multitagResult;
   inline static bool HAS_WARNED = false;
 };
 }  // namespace photon
diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h
@@ -135,7 +135,6 @@ class PhotonTrackedTarget {
   }
 
   bool operator==(const PhotonTrackedTarget& other) const;
-  bool operator!=(const PhotonTrackedTarget& other) const;
 
  private:
   double yaw = 0;