c++ sim rework passing tests

PhotonVision · Nov 25, 2023 · 9399f87 · 9399f87
1 parent 16f63e4
commit 9399f87
Show file tree

Hide file tree

Showing 21 changed files with 2,972 additions and 23 deletions.
diff --git a/build.gradle b/build.gradle
@@ -20,7 +20,7 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2024.1.1-beta-3"
+    wpilibVersion = "2024.1.1-beta-3-53-g31cd015"
     wpimathVersion = wpilibVersion
     openCVversion = "4.8.0-2"
     joglVersion = "2.4.0-rc-20200307"

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;