Update C++ Simulation to match Java (#1026)

PhotonVision · Dec 16, 2023 · cba4db0 · cba4db0
1 parent 47aea29
commit cba4db0
Show file tree

Hide file tree

Showing 42 changed files with 5,123 additions and 15 deletions.
diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
@@ -26,6 +26,7 @@
 
 #include <frc/Errors.h>
 #include <frc/Timer.h>
+#include <opencv2/core.hpp>
 #include <opencv2/core/mat.hpp>
 
 #include "PhotonVersion.h"
@@ -128,8 +129,13 @@ LEDMode PhotonCamera::GetLEDMode() const {
 std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
   auto camCoeffs = cameraIntrinsicsSubscriber.Get();
   if (camCoeffs.size() == 9) {
-    // clone should deal with ownership concerns? not sure
-    return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone();
+    cv::Mat retVal(3, 3, CV_64FC1);
+    for (int i = 0; i < 3; i++) {
+      for (int j = 0; j < 3; j++) {
+        retVal.at<double>(i, j) = camCoeffs[(j * 3) + i];
+      }
+    }
+    return retVal;
   }
   return std::nullopt;
 }
@@ -145,8 +151,11 @@ const std::string_view PhotonCamera::GetCameraName() const {
 std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
   auto distCoeffs = cameraDistortionSubscriber.Get();
   if (distCoeffs.size() == 5) {
-    // clone should deal with ownership concerns? not sure
-    return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone();
+    cv::Mat retVal(5, 1, CV_64FC1);
+    for (int i = 0; i < 5; i++) {
+      retVal.at<double>(i, 0) = distCoeffs[i];
+    }
+    return retVal;
   }
   return std::nullopt;
 }

diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h
@@ -39,11 +39,7 @@
 #include <units/time.h>
 #include <wpi/deprecated.h>
 
-#include "photon/dataflow/structures/Packet.h"
-#include "photon/targeting/MultiTargetPNPResult.h"
-#include "photon/targeting/PNPResult.h"
-#include "photon/targeting/PhotonPipelineResult.h"
-#include "photon/targeting/PhotonTrackedTarget.h"
+#include "photon/targeting//PhotonPipelineResult.h"
 
 namespace cv {
 class Mat;
@@ -172,6 +168,8 @@ class PhotonCamera {
     PhotonCamera::VERSION_CHECK_ENABLED = enabled;
   }
 
+  std::shared_ptr<nt::NetworkTable> GetCameraTable() const { return rootTable; }
+
   // For use in tests
   bool test = false;
   PhotonPipelineResult testResult;