Use ReadQueue for PhotonCamera timestamps (#1316)

This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -74,7 +74,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
       rootTable(mainTable->GetSubTable(cameraName)),
       rawBytesEntry(
           rootTable->GetRawTopic("rawBytes")
-              .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
+              .Subscribe(
+                  "rawBytes", {},
+                  {.pollStorage = 20, .periodic = 0.01, .sendAll = true})),
       inputSaveImgEntry(
           rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
       inputSaveImgSubscriber(
@@ -110,15 +112,15 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
   if (test) {
-    return testResult;
+    if (testResult.size())
+      return testResult.back();
+    else
+      return PhotonPipelineResult{};
   }
 
   // Prints warning if not connected
   VerifyVersion();
 
-  // Clear the current packet.
-  packet.Clear();
-
   // Create the new result;
   PhotonPipelineResult result;
 
@@ -137,6 +139,40 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   return result;
 }
 
+std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
+  if (test) {
+    return testResult;
+  }
+
+  // Prints warning if not connected
+  VerifyVersion();
+
+  const auto changes = rawBytesEntry.ReadQueue();
+
+  // Create the new result list -- these will be updated in-place
+  std::vector<PhotonPipelineResult> ret(changes.size());
+
+  for (size_t i = 0; i < changes.size(); i++) {
+    const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
+
+    if (!value.value.size() || value.time == 0) {
+      continue;
+    }
+
+    // Fill the packet with latest data and populate result.
+    photon::Packet packet{value.value};
+
+    PhotonPipelineResult& result = ret[i];
+    packet >> result;
+    // TODO: NT4 timestamps are still not to be trusted. But it's the best we
+    // can do until we can make time sync more reliable.
+    result.SetRecieveTimestamp(units::microsecond_t(value.time) -
+                               result.GetLatency());
+  }
+
+  return ret;
+}
+
 void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModePublisher.Set(driverMode);
 }

photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp

@@ -70,22 +70,6 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                          frc::Transform3d robotToCamera)
     : aprilTags(tags),
       strategy(strat),
-      camera(nullptr),
-      m_robotToCamera(robotToCamera),
-      lastPose(frc::Pose3d()),
-      referencePose(frc::Pose3d()),
-      poseCacheTimestamp(-1_s) {
-  HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
-             InstanceCount);
-  InstanceCount++;
-}
-
-PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
-                                         PoseStrategy strat, PhotonCamera&& cam,
-                                         frc::Transform3d robotToCamera)
-    : aprilTags(tags),
-      strategy(strat),
-      camera(std::make_shared<PhotonCamera>(std::move(cam))),
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()),
@@ -110,25 +94,6 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
   multiTagFallbackStrategy = strategy;
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
-  if (!camera) {
-    FRC_ReportError(frc::warn::Warning, "[PhotonPoseEstimator] Missing camera!",
-                    "");
-    return std::nullopt;
-  }
-  auto result = camera->GetLatestResult();
-  return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
-}
-
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
-    const PhotonPipelineResult& result) {
-  // If camera is null, best we can do is pass null calibration data
-  if (!camera) {
-    return Update(result, std::nullopt, std::nullopt, this->strategy);
-  }
-  return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
-}
-
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     const PhotonPipelineResult& result,
     std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
@@ -181,11 +146,16 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       ret = AverageBestTargetsStrategy(result);
       break;
     case MULTI_TAG_PNP_ON_COPROCESSOR:
-      ret =
-          MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs);
+      ret = MultiTagOnCoprocStrategy(result);
       break;
     case MULTI_TAG_PNP_ON_RIO:
-      ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
+      if (cameraMatrixData && cameraDistCoeffs) {
+        ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
+      } else {
+        FRC_ReportError(frc::warn::Warning,
+                        "No camera calibration provided to multi-tag-on-rio!",
+                        "");
+      }
       break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -378,9 +348,7 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
-    PhotonPipelineResult result,
-    std::optional<PhotonCamera::CameraMatrix> camMat,
-    std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
+    PhotonPipelineResult result) {
   if (result.MultiTagResult().result.isPresent) {
     const auto field2camera = result.MultiTagResult().result.best;
 
@@ -408,6 +376,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
   }
 
   if (!camMat || !distCoeffs) {
+    FRC_ReportError(frc::warn::Warning,
+                    "No camera calibration data provided to "
+                    "PhotonPoseEstimator::MultiTagOnRioStrategy!",
+                    "");
     return Update(result, std::nullopt, std::nullopt,
                   this->multiTagFallbackStrategy);
   }

photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp

@@ -0,0 +1,392 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "photon/simulation/PhotonCameraSim.h"
+
+#include <algorithm>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace photon {
+PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera)
+    : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {}
+PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
+                                 const SimCameraProperties& props)
+    : prop(props), cam(camera) {
+  SetMinTargetAreaPixels(kDefaultMinAreaPx);
+  videoSimRaw =
+      frc::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
+                                  prop.GetResWidth(), prop.GetResHeight());
+  videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
+  videoSimProcessed = frc::CameraServer::PutVideo(
+      std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
+      prop.GetResHeight());
+  ts.subTable = cam->GetCameraTable();
+  ts.UpdateEntries();
+}
+PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
+                                 const SimCameraProperties& props,
+                                 double minTargetAreaPercent,
+                                 units::meter_t maxSightRange)
+    : PhotonCameraSim(camera, props) {
+  this->minTargetAreaPercent = minTargetAreaPercent;
+  this->maxSightRange = maxSightRange;
+}
+
+bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose,
+                                       const VisionTargetSim& target) {
+  CameraTargetRelation rel{camPose, target.GetPose()};
+  return ((units::math::abs(rel.camToTargYaw.Degrees()) <
+           prop.GetHorizFOV().Degrees() / 2.0) &&
+          (units::math::abs(rel.camToTargPitch.Degrees()) <
+           prop.GetVertFOV().Degrees() / 2.0) &&
+          (!target.GetModel().GetIsPlanar() ||
+           units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
+          (rel.camToTarg.Translation().Norm() <= maxSightRange));
+}
+bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
+  for (const auto& pt : points) {
+    if (std::clamp<float>(pt.x, 0, prop.GetResWidth()) != pt.x ||
+        std::clamp<float>(pt.y, 0, prop.GetResHeight()) != pt.y) {
+      return false;
+    }
+  }
+  return true;
+}
+std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
+  uint64_t now = wpi::Now();
+  uint64_t timestamp{};
+  int iter = 0;
+  while (now >= nextNTEntryTime) {
+    timestamp = nextNTEntryTime;
+    uint64_t frameTime = prop.EstSecUntilNextFrame()
+                             .convert<units::microseconds>()
+                             .to<uint64_t>();
+    nextNTEntryTime += frameTime;
+
+    if (iter++ > 50) {
+      timestamp = now;
+      nextNTEntryTime = now + frameTime;
+      break;
+    }
+  }
+
+  if (timestamp != 0) {
+    return timestamp;
+  } else {
+    return std::nullopt;
+  }
+}
+PhotonPipelineResult PhotonCameraSim::Process(
+    units::second_t latency, const frc::Pose3d& cameraPose,
+    std::vector<VisionTargetSim> targets) {
+  std::sort(targets.begin(), targets.end(),
+            [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
+              units::meter_t dist1 =
+                  t1.GetPose().Translation().Distance(cameraPose.Translation());
+              units::meter_t dist2 =
+                  t2.GetPose().Translation().Distance(cameraPose.Translation());
+              return dist1 > dist2;
+            });
+
+  std::vector<std::pair<VisionTargetSim, std::vector<cv::Point2f>>>
+      visibleTgts{};
+  std::vector<PhotonTrackedTarget> detectableTgts{};
+  RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose);
+
+  VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
+  VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
+  cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
+  cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
+  blankFrame.assignTo(videoSimFrameRaw);
+
+  for (const auto& tgt : targets) {
+    if (!CanSeeTargetPose(cameraPose, tgt)) {
+      continue;
+    }
+
+    std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
+    if (tgt.GetModel().GetIsSpherical()) {
+      TargetModel model = tgt.GetModel();
+      fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
+          tgt.GetPose().Translation(), cameraPose.Translation()));
+    }
+
+    std::vector<cv::Point2f> imagePoints = OpenCVHelp::ProjectPoints(
+        prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners);
+    if (tgt.GetModel().GetIsSpherical()) {
+      cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints);
+      int l = 0;
+      int t = 0;
+      int b = 0;
+      int r = 0;
+      for (int i = 0; i < 4; i++) {
+        if (imagePoints[i].x < imagePoints[l].x) {
+          l = i;
+        }
+      }
+      cv::Point2d lc = imagePoints[l];
+      std::array<double, 4> angles{};
+      t = (l + 1) % 4;
+      b = (l + 1) % 4;
+      for (int i = 0; i < 4; i++) {
+        if (i == l) {
+          continue;
+        }
+        cv::Point2d ic = imagePoints[i];
+        angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x);
+        if (angles[i] >= angles[t]) {
+          t = i;
+        }
+        if (angles[i] <= angles[b]) {
+          b = i;
+        }
+      }
+      for (int i = 0; i < 4; i++) {
+        if (i != t && i != l && i != b) {
+          r = i;
+        }
+      }
+      cv::RotatedRect rect{
+          cv::Point2d{center.x, center.y},
+          cv::Size2d{imagePoints[r].x - lc.x,
+                     imagePoints[b].y - imagePoints[t].y},
+          units::radian_t{-angles[r]}.convert<units::degrees>().to<float>()};
+      std::vector<cv::Point2f> points{};
+      rect.points(points);
+
+      // Can't find an easier way to convert from Point2f to Point2d
+      imagePoints.clear();
+      std::transform(points.begin(), points.end(),
+                     std::back_inserter(imagePoints),
+                     [](const cv::Point2f& p) { return (cv::Point2d)p; });
+    }
+
+    visibleTgts.emplace_back(std::make_pair(tgt, imagePoints));
+    std::vector<cv::Point2f> noisyTargetCorners =
+        prop.EstPixelNoise(imagePoints);
+    cv::RotatedRect minAreaRect =
+        OpenCVHelp::GetMinAreaRect(noisyTargetCorners);
+    std::vector<cv::Point2f> minAreaRectPts;
+    minAreaRectPts.reserve(4);
+    minAreaRect.points(minAreaRectPts);
+    cv::Point2d centerPt = minAreaRect.center;
+    frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
+    double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
+
+    if (!(CanSeeCorner(noisyTargetCorners) &&
+          areaPercent >= minTargetAreaPercent)) {
+      continue;
+    }
+
+    PNPResult pnpSim{};
+    if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
+      pnpSim = OpenCVHelp::SolvePNP_Square(
+          prop.GetIntrinsics(), prop.GetDistCoeffs(),
+          tgt.GetModel().GetVertices(), noisyTargetCorners);
+    }
+
+    std::vector<std::pair<float, float>> tempCorners =
+        OpenCVHelp::PointsToCorners(minAreaRectPts);
+    wpi::SmallVector<std::pair<double, double>, 4> smallVec;
+
+    for (const auto& corner : tempCorners) {
+      smallVec.emplace_back(std::make_pair(static_cast<double>(corner.first),
+                                           static_cast<double>(corner.second)));
+    }
+
+    std::vector<std::pair<float, float>> cornersFloat =
+        OpenCVHelp::PointsToCorners(noisyTargetCorners);
+
+    std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
+                                                         cornersFloat.end()};
+    detectableTgts.emplace_back(
+        -centerRot.Z().convert<units::degrees>().to<double>(),
+        -centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
+        centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
+        tgt.objDetClassId, tgt.objDetConf, pnpSim.best, pnpSim.alt,
+        pnpSim.ambiguity, smallVec, cornersDouble);
+  }
+
+  if (videoSimRawEnabled) {
+    if (videoSimWireframeEnabled) {
+      VideoSimUtil::DrawFieldWireFrame(camRt, prop, videoSimWireframeResolution,
+                                       1.5, cv::Scalar{80}, 6, 1,
+                                       cv::Scalar{30}, videoSimFrameRaw);
+    }
+
+    for (const auto& pair : visibleTgts) {
+      VisionTargetSim tgt = pair.first;
+      std::vector<cv::Point2f> corners = pair.second;
+
+      if (tgt.fiducialId > 0) {
+        VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true,
+                                        videoSimFrameRaw);
+      } else if (!tgt.GetModel().GetIsSpherical()) {
+        std::vector<cv::Point2f> contour = corners;
+        if (!tgt.GetModel().GetIsPlanar()) {
+          contour = OpenCVHelp::GetConvexHull(contour);
+        }
+        VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true,
+                               videoSimFrameRaw);
+      } else {
+        VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255},
+                                           videoSimFrameRaw);
+      }
+    }
+    videoSimRaw.PutFrame(videoSimFrameRaw);
+  } else {
+    videoSimRaw.SetConnectionStrategy(
+        cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
+  }
+
+  if (videoSimProcEnabled) {
+    cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed, cv::COLOR_GRAY2BGR);
+    cv::drawMarker(
+        videoSimFrameProcessed,
+        cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0},
+        cv::Scalar{0, 255, 0}, cv::MARKER_CROSS,
+        static_cast<int>(
+            VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)),
+        static_cast<int>(
+            VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
+        cv::LINE_AA);
+    for (const auto& tgt : detectableTgts) {
+      auto detectedCornersDouble = tgt.GetDetectedCorners();
+      std::vector<std::pair<float, float>> detectedCornerFloat{
+          detectedCornersDouble.begin(), detectedCornersDouble.end()};
+      if (tgt.GetFiducialId() >= 0) {
+        VideoSimUtil::DrawTagDetection(
+            tgt.GetFiducialId(),
+            OpenCVHelp::CornersToPoints(detectedCornerFloat),
+            videoSimFrameProcessed);
+      } else {
+        cv::rectangle(videoSimFrameProcessed,
+                      OpenCVHelp::GetBoundingRect(
+                          OpenCVHelp::CornersToPoints(detectedCornerFloat)),
+                      cv::Scalar{0, 0, 255},
+                      static_cast<int>(VideoSimUtil::GetScaledThickness(
+                          1, videoSimFrameProcessed)),
+                      cv::LINE_AA);
+
+        wpi::SmallVector<std::pair<double, double>, 4> smallVec =
+            tgt.GetMinAreaRectCorners();
+
+        std::vector<std::pair<float, float>> cornersCopy{};
+        cornersCopy.reserve(4);
+
+        for (const auto& corner : smallVec) {
+          cornersCopy.emplace_back(
+              std::make_pair(static_cast<float>(corner.first),
+                             static_cast<float>(corner.second)));
+        }
+
+        VideoSimUtil::DrawPoly(
+            OpenCVHelp::CornersToPoints(cornersCopy),
+            static_cast<int>(
+                VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
+            cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
+      }
+    }
+    videoSimProcessed.PutFrame(videoSimFrameProcessed);
+  } else {
+    videoSimProcessed.SetConnectionStrategy(
+        cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
+  }
+
+  MultiTargetPNPResult multiTagResults{};
+
+  std::vector<frc::AprilTag> visibleLayoutTags =
+      VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
+  if (visibleLayoutTags.size() > 1) {
+    wpi::SmallVector<int16_t, 32> usedIds{};
+    std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
+                   usedIds.begin(),
+                   [](const frc::AprilTag& tag) { return tag.ID; });
+    std::sort(usedIds.begin(), usedIds.end());
+    PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
+        prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
+        kAprilTag36h11);
+    multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
+  }
+
+  heartbeatCounter++;
+  return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts,
+                              multiTagResults};
+}
+void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
+  SubmitProcessedFrame(result, wpi::Now());
+}
+void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
+                                           uint64_t recieveTimestamp) {
+  ts.latencyMillisEntry.Set(
+      result.GetLatency().convert<units::milliseconds>().to<double>(),
+      recieveTimestamp);
+
+  Packet newPacket{};
+  newPacket << result;
+
+  ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
+
+  bool hasTargets = result.HasTargets();
+  ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
+  if (!hasTargets) {
+    ts.targetPitchEntry.Set(0.0, recieveTimestamp);
+    ts.targetYawEntry.Set(0.0, recieveTimestamp);
+    ts.targetAreaEntry.Set(0.0, recieveTimestamp);
+    std::array<double, 3> poseData{0.0, 0.0, 0.0};
+    ts.targetPoseEntry.Set(poseData, recieveTimestamp);
+    ts.targetSkewEntry.Set(0.0, recieveTimestamp);
+  } else {
+    PhotonTrackedTarget bestTarget = result.GetBestTarget();
+
+    ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
+    ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
+    ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
+    ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
+
+    frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
+    std::array<double, 4> poseData{
+        transform.X().to<double>(), transform.Y().to<double>(),
+        transform.Rotation().ToRotation2d().Degrees().to<double>()};
+    ts.targetPoseEntry.Set(poseData, recieveTimestamp);
+  }
+
+  auto intrinsics = prop.GetIntrinsics();
+  std::vector<double> intrinsicsView{intrinsics.data(),
+                                     intrinsics.data() + intrinsics.size()};
+  ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
+
+  auto distortion = prop.GetDistCoeffs();
+  std::vector<double> distortionView{distortion.data(),
+                                     distortion.data() + distortion.size()};
+  ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
+
+  ts.heartbeatPublisher.Set(heartbeatCounter, recieveTimestamp);
+
+  ts.subTable->GetInstance().Flush();
+}
+
+}  // namespace photon

photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp

@@ -0,0 +1,223 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "photon/simulation/SimCameraProperties.h"
+
+#include <algorithm>
+#include <utility>
+#include <vector>
+
+using namespace photon;
+
+void SimCameraProperties::SetCalibration(int width, int height,
+                                         frc::Rotation2d fovDiag) {
+  if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) {
+    fovDiag = frc::Rotation2d{
+        std::clamp<units::degree_t>(fovDiag.Degrees(), 1_deg, 179_deg)};
+    FRC_ReportError(
+        frc::err::Error,
+        "Requested invalid FOV! Clamping between (1, 179) degrees...");
+  }
+  double resDiag = std::sqrt(width * width + height * height);
+  double diagRatio = units::math::tan(fovDiag.Radians() / 2.0);
+  frc::Rotation2d fovWidth{
+      units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}};
+  frc::Rotation2d fovHeight{
+      units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};
+
+  Eigen::Matrix<double, 8, 1> newDistCoeffs =
+      Eigen::Matrix<double, 8, 1>::Zero();
+
+  double cx = width / 2.0 - 0.5;
+  double cy = height / 2.0 - 0.5;
+
+  double fx = cx / units::math::tan(fovWidth.Radians() / 2.0);
+  double fy = cy / units::math::tan(fovHeight.Radians() / 2.0);
+
+  Eigen::Matrix<double, 3, 3> newCamIntrinsics;
+  newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
+  SetCalibration(width, height, newCamIntrinsics, newDistCoeffs);
+}
+
+void SimCameraProperties::SetCalibration(
+    int width, int height, const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
+    const Eigen::Matrix<double, 8, 1>& newDistCoeffs) {
+  resWidth = width;
+  resHeight = height;
+  camIntrinsics = newCamIntrinsics;
+  distCoeffs = newDistCoeffs;
+
+  std::array<frc::Translation3d, 4> p{
+      frc::Translation3d{1_m, frc::Rotation3d{0_rad, 0_rad,
+                                              (GetPixelYaw(0) +
+                                               frc::Rotation2d{units::radian_t{
+                                                   -std::numbers::pi / 2.0}})
+                                                  .Radians()}},
+      frc::Translation3d{1_m, frc::Rotation3d{0_rad, 0_rad,
+                                              (GetPixelYaw(width) +
+                                               frc::Rotation2d{units::radian_t{
+                                                   std::numbers::pi / 2.0}})
+                                                  .Radians()}},
+      frc::Translation3d{1_m, frc::Rotation3d{0_rad,
+                                              (GetPixelPitch(0) +
+                                               frc::Rotation2d{units::radian_t{
+                                                   std::numbers::pi / 2.0}})
+                                                  .Radians(),
+                                              0_rad}},
+      frc::Translation3d{1_m, frc::Rotation3d{0_rad,
+                                              (GetPixelPitch(height) +
+                                               frc::Rotation2d{units::radian_t{
+                                                   -std::numbers::pi / 2.0}})
+                                                  .Radians(),
+                                              0_rad}},
+  };
+  viewplanes.clear();
+  for (size_t i = 0; i < p.size(); i++) {
+    viewplanes.emplace_back(Eigen::Matrix<double, 3, 1>{
+        p[i].X().to<double>(), p[i].Y().to<double>(), p[i].Z().to<double>()});
+  }
+}
+
+std::pair<std::optional<double>, std::optional<double>>
+SimCameraProperties::GetVisibleLine(const RotTrlTransform3d& camRt,
+                                    const frc::Translation3d& a,
+                                    const frc::Translation3d& b) const {
+  frc::Translation3d relA = camRt.Apply(a);
+  frc::Translation3d relB = camRt.Apply(b);
+
+  if (relA.X() <= 0_m && relB.X() <= 0_m) {
+    return {std::nullopt, std::nullopt};
+  }
+
+  Eigen::Matrix<double, 3, 1> av{relA.X().to<double>(), relA.Y().to<double>(),
+                                 relA.Z().to<double>()};
+  Eigen::Matrix<double, 3, 1> bv{relB.X().to<double>(), relB.Y().to<double>(),
+                                 relB.Z().to<double>()};
+  Eigen::Matrix<double, 3, 1> abv = bv - av;
+
+  bool aVisible = true;
+  bool bVisible = true;
+  for (size_t i = 0; i < viewplanes.size(); i++) {
+    Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
+    double aVisibility = av.dot(normal);
+    if (aVisibility < 0) {
+      aVisible = false;
+    }
+    double bVisibility = bv.dot(normal);
+    if (bVisibility < 0) {
+      bVisible = false;
+    }
+    if (aVisibility <= 0 && bVisibility <= 0) {
+      return {std::nullopt, std::nullopt};
+    }
+  }
+
+  if (aVisible && bVisible) {
+    return {0, 1};
+  }
+
+  std::array<double, 4> intersections{std::nan(""), std::nan(""), std::nan(""),
+                                      std::nan("")};
+  std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> ipts{
+      {std::nullopt, std::nullopt, std::nullopt, std::nullopt}};
+
+  for (size_t i = 0; i < viewplanes.size(); i++) {
+    Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
+    Eigen::Matrix<double, 3, 1> a_projn{};
+    a_projn = (av.dot(normal) / normal.dot(normal)) * normal;
+
+    if (std::abs(abv.dot(normal)) < 1e-5) {
+      continue;
+    }
+    intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn));
+
+    Eigen::Matrix<double, 3, 1> apv{};
+    apv = intersections[i] * abv;
+    Eigen::Matrix<double, 3, 1> intersectpt{};
+    intersectpt = av + apv;
+    ipts[i] = intersectpt;
+
+    for (size_t j = 1; j < viewplanes.size(); j++) {
+      int oi = (i + j) % viewplanes.size();
+      Eigen::Matrix<double, 3, 1> onormal = viewplanes[oi];
+      if (intersectpt.dot(onormal) < 0) {
+        intersections[i] = std::nan("");
+        ipts[i] = std::nullopt;
+        break;
+      }
+    }
+
+    if (!ipts[i]) {
+      continue;
+    }
+
+    for (int j = i - 1; j >= 0; j--) {
+      std::optional<Eigen::Matrix<double, 3, 1>> oipt = ipts[j];
+      if (!oipt) {
+        continue;
+      }
+      Eigen::Matrix<double, 3, 1> diff{};
+      diff = oipt.value() - intersectpt;
+      if (diff.cwiseAbs().maxCoeff() < 1e-4) {
+        intersections[i] = std::nan("");
+        ipts[i] = std::nullopt;
+        break;
+      }
+    }
+  }
+
+  double inter1 = std::nan("");
+  double inter2 = std::nan("");
+  for (double inter : intersections) {
+    if (!std::isnan(inter)) {
+      if (std::isnan(inter1)) {
+        inter1 = inter;
+      } else {
+        inter2 = inter;
+      }
+    }
+  }
+
+  if (!std::isnan(inter2)) {
+    double max = std::max(inter1, inter2);
+    double min = std::min(inter1, inter2);
+    if (aVisible) {
+      min = 0;
+    }
+    if (bVisible) {
+      max = 1;
+    }
+    return {min, max};
+  } else if (!std::isnan(inter1)) {
+    if (aVisible) {
+      return {0, inter1};
+    }
+    if (bVisible) {
+      return {inter1, 1};
+    }
+    return {inter1, std::nullopt};
+  } else {
+    return {std::nullopt, std::nullopt};
+  }
+}