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

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

@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <frc/geometry/Pose3d.h>
+
+namespace photon {
+class CameraTargetRelation {
+ public:
+  const frc::Pose3d camPose;
+  const frc::Transform3d camToTarg;
+  const units::meter_t camToTargDist;
+  const units::meter_t camToTargDistXY;
+  const frc::Rotation2d camToTargYaw;
+  const frc::Rotation2d camToTargPitch;
+
+  const frc::Rotation2d camToTargAngle;
+
+  const frc::Transform3d targToCam;
+  const frc::Rotation2d targToCamYaw;
+  const frc::Rotation2d targToCamPitch;
+
+  const frc::Rotation2d targToCamAngle;
+
+  CameraTargetRelation(const frc::Pose3d& cameraPose,
+                       const frc::Pose3d& targetPose)
+      : camPose(cameraPose),
+        camToTarg(frc::Transform3d{cameraPose, targetPose}),
+        camToTargDist(camToTarg.Translation().Norm()),
+        camToTargDistXY(units::math::hypot(camToTarg.Translation().X(),
+                                           camToTarg.Translation().Y())),
+        camToTargYaw(frc::Rotation2d{camToTarg.X().to<double>(),
+                                     camToTarg.Y().to<double>()}),
+        camToTargPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
+                                       -camToTarg.Z().to<double>()}),
+        camToTargAngle(frc::Rotation2d{units::math::hypot(
+            camToTargYaw.Radians(), camToTargPitch.Radians())}),
+        targToCam(frc::Transform3d{targetPose, cameraPose}),
+        targToCamYaw(frc::Rotation2d{targToCam.X().to<double>(),
+                                     targToCam.Y().to<double>()}),
+        targToCamPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
+                                       -targToCam.Z().to<double>()}),
+        targToCamAngle(frc::Rotation2d{units::math::hypot(
+            targToCamYaw.Radians(), targToCamPitch.Radians())}) {}
+};
+}  // namespace photon

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

@@ -0,0 +1,291 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "RotTrlTransform3d.h"
+
+#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
+#include <opencv2/core/eigen.hpp>
+#include "photon/targeting/PNPResult.h"
+#include "photon/targeting/MultiTargetPNPResult.h"
+
+namespace photon {
+namespace OpenCVHelp {
+
+static frc::Rotation3d NWU_TO_EDN{
+    (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()};
+static frc::Rotation3d EDN_TO_NWU{
+    (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()};
+
+static std::vector<cv::Point2f> GetConvexHull(
+    const std::vector<cv::Point2f>& points) {
+  std::vector<int> outputHull{};
+  cv::convexHull(points, outputHull);
+  std::vector<cv::Point2f> convexPoints;
+  for (size_t i = 0; i < outputHull.size(); i++) {
+    convexPoints.push_back(points[outputHull[i]]);
+  }
+  return convexPoints;
+}
+
+static cv::RotatedRect GetMinAreaRect(const std::vector<cv::Point2f>& points) {
+  return cv::minAreaRect(points);
+}
+
+static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) {
+  return trl.RotateBy(NWU_TO_EDN);
+}
+
+static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) {
+  return -NWU_TO_EDN + (rot + NWU_TO_EDN);
+}
+
+static std::vector<cv::Point3f> TranslationToTVec(
+    const std::vector<frc::Translation3d>& translations) {
+  std::vector<cv::Point3f> retVal;
+  retVal.reserve(translations.size());
+  for (size_t i = 0; i < translations.size(); i++) {
+    frc::Translation3d trl = TranslationNWUtoEDN(translations[i]);
+    retVal.emplace_back(cv::Point3f{trl.X().to<float>(), trl.Y().to<float>(),
+                                    trl.Z().to<float>()});
+  }
+  return retVal;
+}
+
+static std::vector<cv::Point3f> RotationToRVec(
+    const frc::Rotation3d& rotation) {
+  std::vector<cv::Point3f> retVal{};
+  frc::Rotation3d rot = RotationNWUtoEDN(rotation);
+  retVal.emplace_back(cv::Point3d{
+      rot.GetQuaternion().ToRotationVector()(0),
+      rot.GetQuaternion().ToRotationVector()(1),
+      rot.GetQuaternion().ToRotationVector()(2),
+  });
+  return retVal;
+}
+
+[[maybe_unused]] static cv::Point2f AvgPoint(std::vector<cv::Point2f> points) {
+  if (points.size() == 0) {
+    return cv::Point2f{};
+  }
+  cv::reduce(points, points, 0, cv::REDUCE_AVG);
+  return points[0];
+}
+
+[[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
+    const std::vector<cv::Point2f>& points) {
+  std::vector<std::pair<float, float>> retVal;
+  retVal.reserve(points.size());
+  for (size_t i = 0; i < points.size(); i++) {
+    retVal.emplace_back(std::make_pair(points[i].x, points[i].y));
+  }
+  return retVal;
+}
+
+[[maybe_unused]] static std::vector<cv::Point2f> CornersToPoints(
+    const std::vector<std::pair<float, float>>& corners) {
+  std::vector<cv::Point2f> retVal;
+  retVal.reserve(corners.size());
+  for (size_t i = 0; i < corners.size(); i++) {
+    retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second});
+  }
+  return retVal;
+}
+
+[[maybe_unused]] static cv::Rect GetBoundingRect(
+    const std::vector<cv::Point2f>& points) {
+  return cv::boundingRect(points);
+}
+
+static std::vector<cv::Point2f> ProjectPoints(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const RotTrlTransform3d& camRt,
+    const std::vector<frc::Translation3d>& objectTranslations) {
+  std::vector<cv::Point3f> objectPoints = TranslationToTVec(objectTranslations);
+  std::vector<cv::Point3f> rvec = RotationToRVec(camRt.GetRotation());
+  std::vector<cv::Point3f> tvec = TranslationToTVec({camRt.GetTranslation()});
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+  std::vector<cv::Point2f> imagePoints{};
+  cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat,
+                    imagePoints);
+  return imagePoints;
+}
+
+template <typename T>
+static std::vector<T> ReorderCircular(const std::vector<T> elements,
+                                      bool backwards, int shiftStart) {
+  size_t size = elements.size();
+  int dir = backwards ? -1 : 1;
+  std::vector<T> reordered{elements};
+  for (size_t i = 0; i < size; i++) {
+    int index = (i * dir + shiftStart * dir) % size;
+    if (index < 0) {
+      index = size + index;
+    }
+    reordered[i] = elements[index];
+  }
+  return reordered;
+}
+
+static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) {
+  return trl.RotateBy(EDN_TO_NWU);
+}
+
+static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) {
+  return -EDN_TO_NWU + (rot + EDN_TO_NWU);
+}
+
+static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) {
+  cv::Vec3f data{};
+  cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F};
+  tvecInput.convertTo(wrapped, CV_32F);
+  data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
+  return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]},
+                                                units::meter_t{data[1]},
+                                                units::meter_t{data[2]}});
+}
+
+static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
+  cv::Vec3f data{};
+  cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F};
+  rvecInput.convertTo(wrapped, CV_32F);
+  data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
+  return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]},
+                                          units::radian_t{data[1]},
+                                          units::radian_t{data[2]}});
+}
+
+[[maybe_unused]] static photon::PNPResult SolvePNP_Square(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    std::vector<frc::Translation3d> modelTrls,
+    std::vector<cv::Point2f> imagePoints) {
+  modelTrls = ReorderCircular(modelTrls, true, -1);
+  imagePoints = ReorderCircular(imagePoints, true, -1);
+  std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
+  std::vector<cv::Mat> rvecs;
+  std::vector<cv::Mat> tvecs;
+  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F);
+
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+
+  cv::Vec2d errors{};
+  frc::Transform3d best{};
+  std::optional<frc::Transform3d> alt{std::nullopt};
+
+  for (int tries = 0; tries < 2; tries++) {
+    cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs,
+                        tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec,
+                        reprojectionError);
+
+    errors = reprojectionError.at<cv::Vec2f>(cv::Point{0, 0});
+    best = frc::Transform3d{TVecToTranslation(tvecs.at(0)),
+                            RVecToRotation(rvecs[0])};
+
+    if (tvecs.size() > 1) {
+      alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)),
+                             RVecToRotation(rvecs[1])};
+    }
+
+    if (!std::isnan(errors[0])) {
+      break;
+    } else {
+      cv::Point2f pt = imagePoints[0];
+      pt.x -= 0.001f;
+      pt.y -= 0.001f;
+      imagePoints[0] = pt;
+    }
+  }
+
+  if (std::isnan(errors[0])) {
+    fmt::print("SolvePNP_Square failed!\n");
+  }
+  if (alt) {
+    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;
+    result.best = best;
+    result.bestReprojErr = errors[0];
+    result.isPresent = true;
+    return result;
+  }
+}
+
+[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    std::vector<frc::Translation3d> modelTrls,
+    std::vector<cv::Point2f> imagePoints) {
+  std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
+  std::vector<cv::Mat> rvecs{};
+  std::vector<cv::Mat> tvecs{};
+  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F);
+  cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F);
+
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+
+  float error = 0;
+  frc::Transform3d best{};
+
+  cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs,
+                      tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec,
+                      reprojectionError);
+
+  error = reprojectionError.at<float>(cv::Point{0, 0});
+  best = frc::Transform3d{TVecToTranslation(tvecs.at(0)),
+                          RVecToRotation(rvecs[0])};
+
+  if (std::isnan(error)) {
+    fmt::print("SolvePNP_Square failed!\n");
+  }
+  photon::PNPResult result;
+  result.best = best;
+  result.bestReprojErr = error;
+  result.isPresent = true;
+  return result;
+}
+}  // namespace OpenCVHelp
+}  // namespace photon

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

@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Translation3d.h>
+
+namespace photon {
+class RotTrlTransform3d {
+ public:
+  RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl)
+      : trl(trl), rot(rot) {}
+  RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last)
+      : trl(last.Translation() - initial.Translation().RotateBy(rot)),
+        rot(last.Rotation() - initial.Rotation()) {}
+  explicit RotTrlTransform3d(const frc::Transform3d& trf)
+      : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {}
+  RotTrlTransform3d()
+      : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {}
+
+  static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) {
+    return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse();
+  }
+
+  RotTrlTransform3d Inverse() const {
+    frc::Rotation3d invRot = -rot;
+    frc::Translation3d invTrl = -(trl.RotateBy(invRot));
+    return RotTrlTransform3d{invRot, invTrl};
+  }
+
+  frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; }
+
+  frc::Translation3d GetTranslation() const { return trl; }
+
+  frc::Rotation3d GetRotation() const { return rot; }
+
+  frc::Translation3d Apply(const frc::Translation3d& trlToApply) const {
+    return trlToApply.RotateBy(rot) + trl;
+  }
+
+  std::vector<frc::Translation3d> ApplyTrls(
+      const std::vector<frc::Translation3d>& trls) const {
+    std::vector<frc::Translation3d> retVal;
+    retVal.reserve(trls.size());
+    for (const auto& currentTrl : trls) {
+      retVal.emplace_back(Apply(currentTrl));
+    }
+    return retVal;
+  }
+
+  frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const {
+    return rotToApply + rot;
+  }
+
+  std::vector<frc::Rotation3d> ApplyTrls(
+      const std::vector<frc::Rotation3d>& rots) const {
+    std::vector<frc::Rotation3d> retVal;
+    retVal.reserve(rots.size());
+    for (const auto& currentRot : rots) {
+      retVal.emplace_back(Apply(currentRot));
+    }
+    return retVal;
+  }
+
+  frc::Pose3d Apply(const frc::Pose3d& poseToApply) const {
+    return frc::Pose3d{Apply(poseToApply.Translation()),
+                       Apply(poseToApply.Rotation())};
+  }
+
+  std::vector<frc::Pose3d> ApplyPoses(
+      const std::vector<frc::Pose3d>& poses) const {
+    std::vector<frc::Pose3d> retVal;
+    retVal.reserve(poses.size());
+    for (const auto& currentPose : poses) {
+      retVal.emplace_back(Apply(currentPose));
+    }
+    return retVal;
+  }
+
+ private:
+  const frc::Translation3d trl;
+  const frc::Rotation3d rot;
+};
+}  // namespace photon

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

@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Translation3d.h>
+
+#include "RotTrlTransform3d.h"
+
+namespace photon {
+class TargetModel {
+ public:
+  TargetModel(units::meter_t width, units::meter_t height)
+      : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0},
+                  frc::Translation3d{0_m, width / 2.0, -height / 2.0},
+                  frc::Translation3d{0_m, width / 2.0, height / 2.0},
+                  frc::Translation3d{0_m, -width / 2.0, height / 2.0}}),
+        isPlanar(true),
+        isSpherical(false) {}
+
+  TargetModel(units::meter_t length, units::meter_t width,
+              units::meter_t height)
+      : TargetModel({
+            frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0},
+            frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0},
+            frc::Translation3d{length / 2.0, width / 2.0, height / 2.0},
+            frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0},
+            frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0},
+            frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0},
+            frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0},
+            frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0},
+        }) {}
+
+  explicit TargetModel(units::meter_t diameter)
+      : vertices({
+            frc::Translation3d{0_m, -diameter / 2.0, 0_m},
+            frc::Translation3d{0_m, 0_m, -diameter / 2.0},
+            frc::Translation3d{0_m, diameter / 2.0, 0_m},
+            frc::Translation3d{0_m, 0_m, diameter / 2.0},
+        }),
+        isPlanar(false),
+        isSpherical(true) {}
+
+  explicit TargetModel(const std::vector<frc::Translation3d>& verts)
+      : isSpherical(false) {
+    if (verts.size() <= 2) {
+      vertices = std::vector<frc::Translation3d>();
+      isPlanar = false;
+    } else {
+      bool cornersPlanar = true;
+      for (const auto& corner : verts) {
+        if (corner.X() != 0_m) {
+          cornersPlanar = false;
+        }
+      }
+      isPlanar = cornersPlanar;
+    }
+    vertices = verts;
+  }
+
+  std::vector<frc::Translation3d> GetFieldVertices(
+      const frc::Pose3d& targetPose) const {
+    RotTrlTransform3d basisChange{targetPose.Rotation(),
+                                  targetPose.Translation()};
+    std::vector<frc::Translation3d> retVal;
+    retVal.reserve(vertices.size());
+    for (const auto& vert : vertices) {
+      retVal.emplace_back(basisChange.Apply(vert));
+    }
+    return retVal;
+  }
+
+  static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl,
+                                     const frc::Translation3d& cameraTrl) {
+    frc::Translation3d relCam = cameraTrl - tgtTrl;
+    frc::Rotation3d orientToCam = frc::Rotation3d{
+        0_rad,
+        frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to<double>(),
+                        -relCam.Z().to<double>()}
+            .Radians(),
+        frc::Rotation2d{relCam.X().to<double>(), relCam.Y().to<double>()}
+            .Radians()};
+    return frc::Pose3d{tgtTrl, orientToCam};
+  }
+
+  std::vector<frc::Translation3d> GetVertices() const { return vertices; }
+  bool GetIsPlanar() const { return isPlanar; }
+  bool GetIsSpherical() const { return isSpherical; }
+
+ private:
+  std::vector<frc::Translation3d> vertices;
+  bool isPlanar;
+  bool isSpherical;
+};
+
+static const TargetModel kAprilTag16h5{6_in, 6_in};
+static const TargetModel kAprilTag36h11{6.5_in, 6.5_in};
+}  // namespace photon

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

@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <frc/apriltag/AprilTag.h>
+#include <frc/apriltag/AprilTagFieldLayout.h>
+
+#include "OpenCVHelp.h"
+#include "TargetModel.h"
+#include "photon/targeting/MultiTargetPNPResult.h"
+#include "photon/targeting/PhotonTrackedTarget.h"
+
+namespace photon {
+namespace VisionEstimation {
+
+static std::vector<frc::AprilTag> GetVisibleLayoutTags(
+    const std::vector<PhotonTrackedTarget>& visTags,
+    const frc::AprilTagFieldLayout& layout) {
+  std::vector<frc::AprilTag> retVal{};
+  for (const auto& tag : visTags) {
+    int id = tag.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      retVal.emplace_back(frc::AprilTag{id, maybePose.value()});
+    }
+  }
+  return retVal;
+}
+
+static PNPResult EstimateCamPosePNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 5, 1>& distCoeffs,
+    const std::vector<PhotonTrackedTarget>& visTags,
+    const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
+  if (visTags.size() == 0) {
+    return PNPResult();
+  }
+
+  std::vector<std::pair<float, float>> corners{};
+  std::vector<frc::AprilTag> knownTags{};
+
+  for (const auto& tgt : visTags) {
+    int id = tgt.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
+      auto currentCorners = tgt.GetDetectedCorners();
+      corners.insert(corners.end(), currentCorners.begin(),
+                     currentCorners.end());
+    }
+  }
+  if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
+    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{};
+    }
+    frc::Pose3d bestPose =
+        knownTags[0].pose.TransformBy(camToTag.best.Inverse());
+    frc::Pose3d altPose{};
+    if (camToTag.ambiguity != 0) {
+      altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse());
+    }
+    frc::Pose3d o{};
+    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;
+    return result;
+  } else {
+    std::vector<frc::Translation3d> objectTrls{};
+    for (const auto& tag : knownTags) {
+      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;
+    }
+  }
+}
+
+}  // namespace VisionEstimation
+}  // namespace photon

photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h

@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/IntegerTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/RawTopic.h>
+
+namespace photon {
+class NTTopicSet {
+ public:
+  std::shared_ptr<nt::NetworkTable> subTable;
+  nt::RawPublisher rawBytesEntry;
+
+  nt::IntegerPublisher pipelineIndexPublisher;
+  nt::IntegerSubscriber pipelineIndexRequestSub;
+
+  nt::BooleanTopic driverModeEntry;
+  nt::BooleanPublisher driverModePublisher;
+  nt::BooleanSubscriber driverModeSubscriber;
+
+  nt::DoublePublisher latencyMillisEntry;
+  nt::BooleanPublisher hasTargetEntry;
+  nt::DoublePublisher targetPitchEntry;
+  nt::DoublePublisher targetYawEntry;
+  nt::DoublePublisher targetAreaEntry;
+  nt::DoubleArrayPublisher targetPoseEntry;
+  nt::DoublePublisher targetSkewEntry;
+
+  nt::DoublePublisher bestTargetPosX;
+  nt::DoublePublisher bestTargetPosY;
+
+  nt::IntegerTopic heartbeatTopic;
+  nt::IntegerPublisher heartbeatPublisher;
+
+  nt::DoubleArrayPublisher cameraIntrinsicsPublisher;
+  nt::DoubleArrayPublisher cameraDistortionPublisher;
+
+  void UpdateEntries() {
+    nt::PubSubOptions options;
+    options.periodic = 0.01;
+    options.sendAll = true;
+    rawBytesEntry =
+        subTable->GetRawTopic("rawBytes").Publish("rawBytes", options);
+
+    pipelineIndexPublisher =
+        subTable->GetIntegerTopic("pipelineIndexState").Publish();
+    pipelineIndexRequestSub =
+        subTable->GetIntegerTopic("pipelineIndexRequest").Subscribe(0);
+
+    driverModePublisher = subTable->GetBooleanTopic("driverMode").Publish();
+    driverModeSubscriber =
+        subTable->GetBooleanTopic("driverModeRequest").Subscribe(0);
+
+    driverModeSubscriber.GetTopic().Publish().SetDefault(false);
+
+    latencyMillisEntry = subTable->GetDoubleTopic("latencyMillis").Publish();
+    hasTargetEntry = subTable->GetBooleanTopic("hasTargets").Publish();
+
+    targetPitchEntry = subTable->GetDoubleTopic("targetPitch").Publish();
+    targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish();
+    targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish();
+    targetPoseEntry = subTable->GetDoubleArrayTopic("targetPose").Publish();
+    targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish();
+
+    bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish();
+    bestTargetPosY = subTable->GetDoubleTopic("targetPixelsY").Publish();
+
+    heartbeatTopic = subTable->GetIntegerTopic("heartbeat");
+    heartbeatPublisher = heartbeatTopic.Publish();
+
+    cameraIntrinsicsPublisher =
+        subTable->GetDoubleArrayTopic("cameraIntrinsics").Publish();
+    cameraDistortionPublisher =
+        subTable->GetDoubleArrayTopic("cameraDistortion").Publish();
+  }
+
+ private:
+};
+}  // namespace photon

photon-targeting/src/main/native/include/photon/targeting/PNPResult.h

@@ -27,15 +27,15 @@ class PNPResult {
  public:
   // This could be wrapped in an std::optional, but chose to do it this way to
   // mirror Java
-  bool isPresent;
+  bool isPresent{false};
 
-  frc::Transform3d best;
-  double bestReprojErr;
+  frc::Transform3d best{};
+  double bestReprojErr{0};
 
-  frc::Transform3d alt;
-  double altReprojErr;
+  frc::Transform3d alt{};
+  double altReprojErr{0};
 
-  double ambiguity;
+  double ambiguity{0};
 
   bool operator==(const PNPResult& other) const;