Merge branch 'master' into sim-draw-targets

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FilterContoursPipe.java

@@ -19,7 +19,6 @@
 
 import java.util.ArrayList;
 import java.util.List;
-
 import org.opencv.core.RotatedRect;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.vision.frame.FrameStaticProperties;
@@ -47,31 +46,13 @@ protected List<Contour> process(List<Contour> in) {
     private void rejectOutliers(List<Contour> list, double xTol, double yTol) {
         if (list.size() < 2) return; // Must have at least 2 points to reject outliers
 
-/*
-        // Sort by X and find median
-        list.sort(Comparator.comparingDouble(c -> c.getCenterPoint().x));
-
-        double medianX = list.get(list.size() / 2).getCenterPoint().x;
-        if (list.size() % 2 == 0)
-            medianX = (medianX + list.get(list.size() / 2 - 1).getCenterPoint().x) / 2;
-*/
-
         double meanX = list.stream().mapToDouble(it -> it.getCenterPoint().x).sum() / list.size();
 
         double stdDevX =
                 list.stream().mapToDouble(it -> Math.pow(it.getCenterPoint().x - meanX, 2.0)).sum();
         stdDevX /= (list.size() - 1);
         stdDevX = Math.sqrt(stdDevX);
 
-/*
-        // Sort by Y and find median
-        list.sort(Comparator.comparingDouble(c -> c.getCenterPoint().y));
-
-        double medianY = list.get(list.size() / 2).getCenterPoint().y;
-        if (list.size() % 2 == 0)
-            medianY = (medianY + list.get(list.size() / 2 - 1).getCenterPoint().y) / 2;
-*/
-
         double meanY = list.stream().mapToDouble(it -> it.getCenterPoint().y).sum() / list.size();
 
         double stdDevY =
@@ -112,7 +93,8 @@ private void filterContour(Contour contour) {
         if (contourArea <= minFullness || contourArea >= maxFullness) return;
 
         // Aspect Ratio Filtering.
-        double aspectRatio = TargetCalculations.getAspectRatio(contour.getMinAreaRect(), params.isLandscape);
+        double aspectRatio =
+                TargetCalculations.getAspectRatio(contour.getMinAreaRect(), params.isLandscape);
         if (aspectRatio < params.getRatio().getFirst() || aspectRatio > params.getRatio().getSecond())
             return;
 
@@ -134,7 +116,8 @@ public FilterContoursParams(
                 DoubleCouple extent,
                 FrameStaticProperties camProperties,
                 double xTol,
-                double yTol, boolean isLandscape) {
+                double yTol,
+                boolean isLandscape) {
             this.m_area = area;
             this.m_ratio = ratio;
             this.m_fullness = extent;

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

@@ -115,9 +115,7 @@ void PhotonCamera::TakeOutputSnapshot() {
 
 bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
 
-void PhotonCamera::SetPipelineIndex(int index) {
-  pipelineIndexPub.Set(static_cast<double>(index));
-}
+void PhotonCamera::SetPipelineIndex(int index) { pipelineIndexPub.Set(index); }
 
 int PhotonCamera::GetPipelineIndex() const {
   return static_cast<int>(pipelineIndexSub.Get());
@@ -137,7 +135,7 @@ std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
 }
 
 void PhotonCamera::SetLEDMode(LEDMode mode) {
-  ledModePub.Set(static_cast<double>(static_cast<int>(mode)));
+  ledModePub.Set(static_cast<int>(mode));
 }
 
 const std::string_view PhotonCamera::GetCameraName() const {

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

@@ -115,7 +115,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
-    std::optional<cv::Mat> coeffsData) {
+    std::optional<cv::Mat> cameraDistCoeffs) {
   // Time in the past -- give up, since the following if expects times > 0
   if (result.GetTimestamp() < 0_s) {
     return std::nullopt;
@@ -136,12 +136,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     return std::nullopt;
   }
 
-  return Update(result, cameraMatrixData, coeffsData, this->strategy);
+  return Update(result, cameraMatrixData, cameraDistCoeffs, this->strategy);
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
-    std::optional<cv::Mat> coeffsData, PoseStrategy strategy) {
+    std::optional<cv::Mat> cameraDistCoeffs, PoseStrategy strategy) {
   std::optional<EstimatedRobotPose> ret = std::nullopt;
 
   switch (strategy) {
@@ -162,7 +162,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       ret = AverageBestTargetsStrategy(result);
       break;
     case ::photonlib::MULTI_TAG_PNP:
-      ret = MultiTagPnpStrategy(result, coeffsData, cameraMatrixData);
+      ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs);
       break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -396,14 +396,14 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
     return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy);
   }
 
-  // Use OpenCV ITERATIVE solver
+  // Output mats for results
   cv::Mat const rvec(3, 1, cv::DataType<double>::type);
   cv::Mat const tvec(3, 1, cv::DataType<double>::type);
 
   cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
                rvec, tvec, false, cv::SOLVEPNP_SQPNP);
 
-  Pose3d const pose = detail::ToPose3d(tvec, rvec);
+  const Pose3d pose = detail::ToPose3d(tvec, rvec);
 
   return photonlib::EstimatedRobotPose(
       pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),