diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java index e5407baac..9f082e921 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java @@ -263,12 +263,15 @@ public class StandardCVPipeline extends CVPipeline leftRightDualTargetPair = null; public Pair leftRightRotatedRect = null; + public MatOfPoint2f rawContour = kMat2f; public void release() { rVector.release(); tVector.release(); imageCornerPoints.release(); } + + private static final MatOfPoint2f kMat2f = new MatOfPoint2f(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java index e0a9d9e2a..49e4b84e7 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java @@ -45,6 +45,7 @@ public class GroupContoursPipe implements Pipe, List (group.equals(TargetGroup.Single) ? 0 : 1)) { @@ -61,6 +62,7 @@ public class GroupContoursPipe implements Pipe, List, List Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); private double distanceDivisor = 1.0; Mat scaledTvec = new Mat(); + private Comparator distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) { super(); @@ -121,7 +122,36 @@ public class SolvePNPPipe implements Pipe return points; } - private MatOfPoint2f findCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { + /** + * Find the target using the outermost tape corners and a 2020 target. + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target) { + if(target.rawContour.cols() < 1) return null; + + var centroid = target.minAreaRect.center; + var contour = target.rawContour; + var combinedList = contour.toList(); + + // start looking in the top left quadrant + var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); + var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); + var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); + var br = combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); + + return boundingBoxResultMat; + } + + /** + * Find the target using the outermost tape corners and a dual target. + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { if(target.leftRightRotatedRect == null) return null; var centroid = target.minAreaRect.center; @@ -144,8 +174,6 @@ public class SolvePNPPipe implements Pipe combinedList.addAll(List.of(rightPoints)); // start looking in the top left quadrant - Comparator distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); - var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();