Update cpp

PhotonVision · Nov 24, 2023 · ca15e66 · ca15e66
1 parent 0f0727c
commit ca15e66
Show file tree

Hide file tree

Showing 14 changed files with 47 additions and 158 deletions.
diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -360,8 +360,8 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
     PhotonPipelineResult result, std::optional<cv::Mat> camMat,
     std::optional<cv::Mat> distCoeffs) {
-  if (result.MultiTagResult().result.isPresent) {
-    const auto field2camera = result.MultiTagResult().result.best;
+  if (result.MultiTagResult().has_value()) {
+    const auto field2camera = result.MultiTagResult().value().best;
 
     const auto fieldToRobot =
         frc::Pose3d() + field2camera + m_robotToCamera.Inverse();

diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h
@@ -40,7 +40,6 @@
 #include <wpi/deprecated.h>
 
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
@@ -32,7 +32,6 @@
 
 #include "photon/PhotonCamera.h"
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h
@@ -27,7 +27,6 @@
 #include <cmath>
 
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-lib/src/main/native/include/photon/PhotonUtils.h b/photon-lib/src/main/native/include/photon/PhotonUtils.h
@@ -33,7 +33,6 @@
 #include <units/math.h>
 
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h
@@ -35,7 +35,6 @@
 #include "photon/PhotonCamera.h"
 #include "photon/PhotonTargetSortMode.h"
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h
@@ -35,7 +35,6 @@
 #include "SimPhotonCamera.h"
 #include "SimVisionTarget.h"
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h
@@ -28,7 +28,6 @@
 #include <units/area.h>
 
 #include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 

diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp
@@ -22,7 +22,10 @@
 namespace photon {
 
 bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const {
-  return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed;
+  return other.best == best &&
+         other.bestReprojErr == bestReprojErr && other.alt == alt &&
+         other.altReprojErr == altReprojErr && other.ambiguity == ambiguity
+         && other.fiducialIdsUsed == fiducialIdsUsed;
 }
 }  // namespace photon
 
@@ -45,15 +48,23 @@ wpi::Protobuf<photon::MultiTargetPNPResult>::Unpack(
   }
 
   return photon::MultiTargetPNPResult{
-      wpi::UnpackProtobuf<photon::PNPResult>(m->estimated_pose()),
+      wpi::UnpackProtobuf<frc::Transform3d>(m->best()),
+      m->best_reproj_err(),
+      wpi::UnpackProtobuf<frc::Transform3d>(m->alt()),
+      m->alt_reproj_err(),
+      m->ambiguity(),
       fiducialIdsUsed};
 }
 
 void wpi::Protobuf<photon::MultiTargetPNPResult>::Pack(
     google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) {
   auto m = static_cast<photonvision::proto::ProtobufMultiTargetPNPResult*>(msg);
 
-  wpi::PackProtobuf(m->mutable_estimated_pose(), value.result);
+  wpi::PackProtobuf(m->mutable_best(), value.best);
+  m->set_best_reproj_err(value.bestReprojErr);
+  wpi::PackProtobuf(m->mutable_alt(), value.alt);
+  m->set_alt_reproj_err(value.altReprojErr);
+  m->set_ambiguity(value.ambiguity);
 
   m->clear_fiducial_ids_used();
   for (const auto& t : value.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
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp
@@ -45,10 +45,13 @@ wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
     targets.emplace_back(wpi::UnpackProtobuf<photon::PhotonTrackedTarget>(t));
   }
 
-  return photon::PhotonPipelineResult{
-      units::millisecond_t{m->latency_ms()}, targets,
-      wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
-          m->multi_target_result())};
+  if(m->has_multi_target_result()) {
+    return photon::PhotonPipelineResult{
+        units::millisecond_t{m->latency_ms()}, targets,
+        wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(m->multi_target_result())};
+  } else {
+    return photon::PhotonPipelineResult{units::millisecond_t{m->latency_ms()}, targets};
+  }
 }
 
 void wpi::Protobuf<photon::PhotonPipelineResult>::Pack(
@@ -62,5 +65,7 @@ void wpi::Protobuf<photon::PhotonPipelineResult>::Pack(
     wpi::PackProtobuf(m->add_targets(), t);
   }
 
-  wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult);
+  if(value.multitagResult.has_value()) {
+    wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult.value());
+  }
 }
diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h
@@ -21,19 +21,29 @@
 #include <wpi/SmallVector.h>
 #include <wpi/protobuf/Protobuf.h>
 
-#include "PNPResult.h"
-
 namespace photon {
 class MultiTargetPNPResult {
  public:
-  PNPResult result;
-  wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
+  frc::Transform3d best;
+  double bestReprojErr;
+
+  frc::Transform3d alt;
+  double altReprojErr;
 
-  MultiTargetPNPResult() = default;
+  double ambiguity;
+
+  wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
 
-  MultiTargetPNPResult(PNPResult result,
+  MultiTargetPNPResult(frc::Transform3d best, double bestReprojErr,
+                       frc::Transform3d alt, double altReprojErr,
+                       double ambiguity,
                        wpi::SmallVector<int16_t, 32> fiducialIdsUsed)
-      : result(result), fiducialIdsUsed(fiducialIdsUsed) {}
+      : best(best),
+        bestReprojErr(bestReprojErr),
+        alt(alt),
+        altReprojErr(altReprojErr),
+        ambiguity(ambiguity),
+        fiducialIdsUsed(fiducialIdsUsed) {}
 
   bool operator==(const MultiTargetPNPResult& other) const;
 };

diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h
diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h
@@ -19,6 +19,7 @@
 
 #include <span>
 #include <string>
+#include <optional>
 
 #include <frc/Errors.h>
 #include <units/time.h>
@@ -37,7 +38,7 @@ class PhotonPipelineResult {
   units::millisecond_t latency = 0_s;
   units::second_t timestamp = -1_s;
   wpi::SmallVector<PhotonTrackedTarget, 10> targets;
-  MultiTargetPNPResult multitagResult;
+  std::optional<MultiTargetPNPResult> multitagResult;
   inline static bool HAS_WARNED = false;
 
   /**
@@ -103,11 +104,9 @@ 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.isPresent before using
-   * the pose estimate!
+   * Return the MultiTarget Result. Empty if disabled or unable to create result.
    */
-  const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; }
+  std::optional<MultiTargetPNPResult> MultiTagResult() { return multitagResult; }
 
   /**
    * Sets the timestamp in seconds