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 2020 vision code

Browse Source
This commit is contained in:
Matt
2020-01-04 22:48:49 -08:00
parent c32169804f
commit 5c4acc411d
3 changed files with 41 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -263,12 +263,15 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
public Pair<Rect, Rect> leftRightDualTargetPair = null;
public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
public MatOfPoint2f rawContour = kMat2f;
public void release() {
rVector.release();
tVector.release();
imageCornerPoints.release();
}
private static final MatOfPoint2f kMat2f = new MatOfPoint2f();
}

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

@@ -45,6 +45,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
groupedContours.forEach(StandardCVPipeline.TrackedTarget::release);
groupedContours.clear();
contourBuffer.release();
if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
@@ -61,6 +62,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.rawContour = contourBuffer;
groupedContours.add(target);
}
});
@@ -91,18 +93,21 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
// find left and right bouding rectangles
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),
Imgproc.boundingRect(secondContour));
// find left and right min area rectangles
tempRectMat.fromArray(firstContour.toArray());
var minAreaRect1 = Imgproc.minAreaRect(tempRectMat);
tempRectMat.fromArray(secondContour.toArray());
var minAreaRect2 = Imgproc.minAreaRect(tempRectMat);
target.leftRightRotatedRect =
Pair.of(minAreaRect1, minAreaRect2);
target.rawContour = contourBuffer;
groupedContours.add(target);
firstContour.release();

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

@@ -37,6 +37,7 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
private double distanceDivisor = 1.0;
Mat scaledTvec = new Mat();
private Comparator<Point> 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<List<StandardCVPipeline.TrackedTarget>
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<List<StandardCVPipeline.TrackedTarget>
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();