diff --git a/chameleon-client/src/assets/FRCtargets.json b/chameleon-client/src/assets/FRCtargets.json index 55aa10fb6..0b8b2841f 100644 --- a/chameleon-client/src/assets/FRCtargets.json +++ b/chameleon-client/src/assets/FRCtargets.json @@ -3,7 +3,11 @@ [-19.625, 0], [-9.819867, -17], [9.819867, -17], - [19.625,0] + [19.625,0], + [17.316, 0], + [8.665867, -15], + [-9.819867, -15], + [-17.316, 0] ], "2020 Loading Station": [ [-3.54,5.5], diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java index fd4d301bb..373c1a41c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java @@ -162,11 +162,11 @@ public class SolvePNPPipe implements Pipe distanceBetween(p, boundingBoxCorners.get(0)))).get(); - Point tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); + var tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get(); + var tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); - -// var tl = polyList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); -// var tr = polyList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); var bl = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); var br = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + polyList = new ArrayList<>(polyList); + polyList.removeAll(List.of(tl, tr, bl, br)); + + var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get(); + var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); + + var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); + var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + target2020ResultMat.release(); - target2020ResultMat.fromList(List.of(tl, bl, br, tr)); + target2020ResultMat.fromList(List.of(tl, bl, br, tr, tr2, br2, bl2, tl2)); return target2020ResultMat; } catch (NoSuchElementException e) {