Auto-generate packet dataclasses with Jinja (#1374)

photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h

@@ -29,9 +29,11 @@
 
 #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
 #include <opencv2/core/eigen.hpp>
-#include "photon/targeting/PNPResult.h"
+#include "photon/targeting/PnpResult.h"
 #include "photon/targeting/MultiTargetPNPResult.h"
 
+#include "photon/targeting/TargetCorner.h"
+
 namespace photon {
 namespace OpenCVHelp {
 
@@ -96,6 +98,16 @@ static std::vector<cv::Point3f> RotationToRVec(
   return points[0];
 }
 
+[[maybe_unused]] static std::vector<photon::TargetCorner> PointsToTargetCorners(
+    const std::vector<cv::Point2f>& points) {
+  std::vector<photon::TargetCorner> retVal;
+  retVal.reserve(points.size());
+  for (size_t i = 0; i < points.size(); i++) {
+    retVal.emplace_back(photon::TargetCorner{points[i].x, points[i].y});
+  }
+  return retVal;
+}
+
 [[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
     const std::vector<cv::Point2f>& points) {
   std::vector<std::pair<float, float>> retVal;
@@ -116,6 +128,17 @@ static std::vector<cv::Point3f> RotationToRVec(
   return retVal;
 }
 
+[[maybe_unused]] static std::vector<cv::Point2f> CornersToPoints(
+    const std::vector<photon::TargetCorner>& corners) {
+  std::vector<cv::Point2f> retVal;
+  retVal.reserve(corners.size());
+  for (size_t i = 0; i < corners.size(); i++) {
+    retVal.emplace_back(cv::Point2f{static_cast<float>(corners[i].x),
+                                    static_cast<float>(corners[i].y)});
+  }
+  return retVal;
+}
+
 [[maybe_unused]] static cv::Rect GetBoundingRect(
     const std::vector<cv::Point2f>& points) {
   return cv::boundingRect(points);
@@ -184,7 +207,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
                                           units::radian_t{data[2]}});
 }
 
-[[maybe_unused]] static photon::PNPResult SolvePNP_Square(
+[[maybe_unused]] static std::optional<photon::PnpResult> SolvePNP_Square(
     const Eigen::Matrix<double, 3, 3>& cameraMatrix,
     const Eigen::Matrix<double, 8, 1>& distCoeffs,
     std::vector<frc::Translation3d> modelTrls,
@@ -233,26 +256,25 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
 
   if (std::isnan(errors[0])) {
     fmt::print("SolvePNP_Square failed!\n");
+    return std::nullopt;
   }
   if (alt) {
-    photon::PNPResult result;
+    photon::PnpResult result;
     result.best = best;
     result.alt = alt.value();
     result.ambiguity = errors[0] / errors[1];
     result.bestReprojErr = errors[0];
     result.altReprojErr = errors[1];
-    result.isPresent = true;
     return result;
   } else {
-    photon::PNPResult result;
+    photon::PnpResult result;
     result.best = best;
     result.bestReprojErr = errors[0];
-    result.isPresent = true;
     return result;
   }
 }
 
-[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP(
+[[maybe_unused]] static std::optional<photon::PnpResult> SolvePNP_SQPNP(
     const Eigen::Matrix<double, 3, 3>& cameraMatrix,
     const Eigen::Matrix<double, 8, 1>& distCoeffs,
     std::vector<frc::Translation3d> modelTrls,
@@ -283,10 +305,9 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
   if (std::isnan(error)) {
     fmt::print("SolvePNP_Square failed!\n");
   }
-  photon::PNPResult result;
+  photon::PnpResult result;
   result.best = best;
   result.bestReprojErr = error;
-  result.isPresent = true;
   return result;
 }
 }  // namespace OpenCVHelp

photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h

@@ -47,16 +47,18 @@ static std::vector<frc::AprilTag> GetVisibleLayoutTags(
   return retVal;
 }
 
-static PNPResult EstimateCamPosePNP(
+#include <iostream>
+
+static std::optional<PnpResult> EstimateCamPosePNP(
     const Eigen::Matrix<double, 3, 3>& cameraMatrix,
     const Eigen::Matrix<double, 8, 1>& distCoeffs,
     const std::vector<PhotonTrackedTarget>& visTags,
     const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
   if (visTags.size() == 0) {
-    return PNPResult();
+    return PnpResult();
   }
 
-  std::vector<std::pair<float, float>> corners{};
+  std::vector<photon::TargetCorner> corners{};
   std::vector<frc::AprilTag> knownTags{};
 
   for (const auto& tgt : visTags) {
@@ -70,30 +72,30 @@ static PNPResult EstimateCamPosePNP(
     }
   }
   if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
-    return PNPResult{};
+    return PnpResult{};
   }
 
   std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
 
   if (knownTags.size() == 1) {
-    PNPResult camToTag = OpenCVHelp::SolvePNP_Square(
-        cameraMatrix, distCoeffs, tagModel.GetVertices(), points);
-    if (!camToTag.isPresent) {
-      return PNPResult{};
+    auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs,
+                                                tagModel.GetVertices(), points);
+    if (!camToTag) {
+      return PnpResult{};
     }
     frc::Pose3d bestPose =
-        knownTags[0].pose.TransformBy(camToTag.best.Inverse());
+        knownTags[0].pose.TransformBy(camToTag->best.Inverse());
     frc::Pose3d altPose{};
-    if (camToTag.ambiguity != 0) {
-      altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse());
+    if (camToTag->ambiguity != 0) {
+      altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse());
     }
     frc::Pose3d o{};
-    PNPResult result{};
+    PnpResult result{};
     result.best = frc::Transform3d{o, bestPose};
     result.alt = frc::Transform3d{o, altPose};
-    result.ambiguity = camToTag.ambiguity;
-    result.bestReprojErr = camToTag.bestReprojErr;
-    result.altReprojErr = camToTag.altReprojErr;
+    result.ambiguity = camToTag->ambiguity;
+    result.bestReprojErr = camToTag->bestReprojErr;
+    result.altReprojErr = camToTag->altReprojErr;
     return result;
   } else {
     std::vector<frc::Translation3d> objectTrls{};
@@ -101,20 +103,15 @@ static PNPResult EstimateCamPosePNP(
       auto verts = tagModel.GetFieldVertices(tag.pose);
       objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
     }
-    PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs,
-                                                       objectTrls, points);
-    if (!camToOrigin.isPresent) {
-      return PNPResult{};
-    } else {
-      PNPResult result{};
-      result.best = camToOrigin.best.Inverse(),
-      result.alt = camToOrigin.alt.Inverse(),
-      result.ambiguity = camToOrigin.ambiguity;
-      result.bestReprojErr = camToOrigin.bestReprojErr;
-      result.altReprojErr = camToOrigin.altReprojErr;
-      result.isPresent = true;
-      return result;
+    auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
+                                          points);
+    if (ret) {
+      // Invert best/alt transforms
+      ret->best = ret->best.Inverse();
+      ret->alt = ret->alt.Inverse();
     }
+
+    return ret;
   }
 }