Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add method to find most extreme per-quadrant points for solvePNP

Browse Source
This commit is contained in:
Matt
2020-01-02 09:53:44 -08:00
parent bf54a2d53f
commit 844d10335c
3 changed files with 44 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
View File

@@ -253,6 +253,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public Mat tVector = new Mat();
public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
public Pair<Rect, Rect> leftRightDualTargetPair = null;
public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
}

5
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
View File

@@ -91,10 +91,15 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.contour = contour;
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),
Imgproc.boundingRect(secondContour));
target.leftRightRotatedRect =
Pair.of(Imgproc.minAreaRect(new MatOfPoint2f(firstContour.toArray())),
Imgproc.minAreaRect(new MatOfPoint2f(secondContour.toArray())));
groupedContours.add(target);
}
} catch (IndexOutOfBoundsException e) {

39
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java
View File

@@ -120,6 +120,42 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
return points;
}
private MatOfPoint2f findCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) {
if(target.leftRightRotatedRect == null) return null;
var centroid = target.minAreaRect.center;
var left = target.leftRightRotatedRect.getLeft();
var right = target.leftRightRotatedRect.getRight();
// flip if the "left" target is to the right
if(left.center.x > right.center.x) {
var temp = left;
left = right;
right = temp;
}
var leftPoints = new Point[4];
left.points(leftPoints);
var rightPoints = new Point[4];
right.points(rightPoints);
ArrayList<Point> combinedList = new ArrayList<>(List.of(leftPoints));
combinedList.addAll(List.of(rightPoints));
// start looking in the top left quadrant
Comparator<Point> 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();
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;
}
private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
// List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
@@ -202,7 +238,8 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
return tempCornerList;
}
private MatOfPoint2f findCorners2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
@Deprecated
private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
// Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);