Fix solvePNP draw bug (#172)

Removes duplicate members, try-catches streaming processing, and prevents drawing empty contours. The latter likely stems from some sort of use-after-free condition.

photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -26,6 +26,8 @@ import org.opencv.core.Mat;
 import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.imgproc.Imgproc;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ColorHelper;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.MutatingPipe;
@@ -35,6 +37,8 @@ import org.photonvision.vision.target.TrackedTarget;
 public class Draw3dTargetsPipe
         extends MutatingPipe<Pair<Mat, List<TrackedTarget>>, Draw3dTargetsPipe.Draw3dContoursParams> {
 
+    Logger logger = new Logger(Draw3dTargetsPipe.class, LogGroup.VisionModule);
+
     @Override
     protected Void process(Pair<Mat, List<TrackedTarget>> in) {
         if (!params.shouldDraw) return null;
@@ -44,6 +48,12 @@ public class Draw3dTargetsPipe
             // draw convex hull
             var pointMat = new MatOfPoint();
             target.m_mainContour.getConvexHull().convertTo(pointMat, CvType.CV_32S);
+            if (pointMat.size().empty()) {
+                logger.error("Convex hull is empty?");
+                logger.debug(
+                        "Orig. Convex Hull: " + target.m_mainContour.getConvexHull().size().toString());
+                continue;
+            }
             Imgproc.drawContours(
                     in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.green), 1);
 

photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java

@@ -18,14 +18,12 @@
 package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.annotation.JsonTypeName;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import java.util.Objects;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.opencv.ContourGroupingMode;
 import org.photonvision.vision.opencv.ContourIntersectionDirection;
 import org.photonvision.vision.opencv.ContourShape;
 import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
-import org.photonvision.vision.target.TargetModel;
 
 @JsonTypeName("ColoredShapePipelineSettings")
 public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
@@ -49,10 +47,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
     public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
 
     // 3d settings
-    public boolean solvePNPEnabled = false;
     public CameraCalibrationCoefficients cameraCalibration;
-    public TargetModel targetModel;
-    public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0); // TODO where should pitch live?
 
     // Corner detection settings
     public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
@@ -98,7 +93,6 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
                 && contourIntersection == that.contourIntersection
                 && Objects.equals(cameraCalibration, that.cameraCalibration)
                 && Objects.equals(targetModel, that.targetModel)
-                && Objects.equals(cameraPitch, that.cameraPitch)
                 && cornerDetectionStrategy == that.cornerDetectionStrategy
                 && erode == that.erode
                 && dilate == that.dilate;
@@ -125,7 +119,6 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
                 solvePNPEnabled,
                 cameraCalibration,
                 targetModel,
-                cameraPitch,
                 cornerDetectionStrategy,
                 cornerDetectionUseConvexHulls,
                 cornerDetectionExactSideCount,

photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java

@@ -21,7 +21,6 @@ import com.fasterxml.jackson.annotation.JsonTypeName;
 import org.photonvision.vision.opencv.ContourGroupingMode;
 import org.photonvision.vision.opencv.ContourIntersectionDirection;
 import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
-import org.photonvision.vision.target.TargetModel;
 
 @JsonTypeName("ReflectivePipelineSettings")
 public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
@@ -31,10 +30,6 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
     // the direction in which contours must intersect to be considered intersecting
     public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
 
-    // 3d settings
-    public boolean solvePNPEnabled = false;
-    public TargetModel targetModel = TargetModel.k2020HighGoalOuter;
-
     // Corner detection settings
     public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
             CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;

photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -252,10 +252,15 @@ public class VisionModule {
                     this.shouldRun = false;
                 }
                 if (shouldRun) {
-                    var osr = outputStreamPipeline.process(inputFrame, outputFrame, settings, targets);
-                    consumeFpsLimitedResult(osr);
-                    inputFrame.release();
-                    outputFrame.release();
+                    try {
+                        var osr = outputStreamPipeline.process(inputFrame, outputFrame, settings, targets);
+                        consumeFpsLimitedResult(osr);
+                        inputFrame.release();
+                        outputFrame.release();
+                    } catch (Exception e) {
+                        // Never die
+                        logger.error("Exception in stream runnable!", e);
+                    }
                 } else {
                     // busy wait! hurray!
                     try {