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 new corner detection algorithm for solvePNP

Browse Source
This commit is contained in:
Matt
2020-01-12 23:26:35 -08:00
parent b2e44e448c
commit a6654fa24b
3 changed files with 55 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
chameleon-server/src/main/java/com/chameleonvision/util/Helpers.java
View File

@@ -12,7 +12,7 @@ public class Helpers {
}
public static Scalar colorToScalar(Color color) {
return new Scalar(color.getRed(), color.getGreen(), color.getBlue());
return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
}
public static HashMap VideoModeToHashMap(VideoMode videoMode) {

19
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
View File

@@ -16,10 +16,12 @@ import java.util.List;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
private MatOfPoint tempMatOfPoints = new MatOfPoint();
public Scalar green = Helpers.colorToScalar(Color.GREEN);
public Scalar blue = Helpers.colorToScalar(Color.BLUE);
public Scalar red = Helpers.colorToScalar(Color.RED);
public Scalar orange = Helpers.colorToScalar(Color.orange);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
@@ -97,17 +99,20 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2);
}
// draw corners
for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
var point = new Point(it.imageCornerPoints.get(i, 0));
Imgproc.circle(image, point, 4, green, 5);
}
// draw poly dp
var list = it.approxPoly.toList();
for(int i = 0; i < list.size(); i++) {
var next = (i == list.size() - 1) ? list.get(0) : list.get(i + 1);
Imgproc.line(image, list.get(i), next, red, 3);
Imgproc.line(image, list.get(i), next, red, 2);
}
// draw center
Imgproc.circle(image, it.minAreaRect.center, 5, red);
// draw corners
for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
var point = new Point(it.imageCornerPoints.get(i, 0));
Imgproc.circle(image, point, 4, green, 5);
}
// sketch out floor

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

@@ -137,6 +137,16 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return points;
}
MatOfPoint2f target2020ResultMat = new MatOfPoint2f();
private double distanceBetween(Point a, Point b) {
return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
}
MatOfInt tempInt = new MatOfInt();
MatOfPoint2f tempMat2f = new MatOfPoint2f();
MatOfPoint tempMatOfPoint = new MatOfPoint();
/**
* Find the target using the outermost tape corners and a 2020 target.
* @param target the target.
@@ -149,25 +159,45 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
var contour = target.rawContour;
var convex = tempInt;
tempMatOfPoint.fromList(contour.toList());
Imgproc.convexHull(tempMatOfPoint, convex);
var combinedList = contour.toList();
// approx poly dp time
Imgproc.approxPolyDP(contour, polyOutput, 3, true);
Point[] contourArray = contour.toArray();
Point[] hullPoints = new Point[convex.rows()];
List<Integer> hullContourIdxList = convex.toList();
for (int i = 0; i < hullContourIdxList.size(); i++) {
hullPoints[i] = contourArray[hullContourIdxList.get(i)];
}
tempMat2f.fromArray(hullPoints);
Imgproc.approxPolyDP(tempMat2f, polyOutput, 5, true);
var polyList = polyOutput.toList();
polyOutput.copyTo(target.approxPoly);
// start looking in the top left quadrant
// left top, left bottom, right bottom, right top
var boundingBoxCorners = findBoundingBoxCorners(target).toList();
try {
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();
// top left and top right are the poly corners closest to the bouding box tl and tr
Point tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> 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().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();
boundingBoxResultMat.release();
boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
target2020ResultMat.release();
target2020ResultMat.fromList(List.of(tl, bl, br, tr));
return boundingBoxResultMat;
return target2020ResultMat;
} catch (NoSuchElementException e) {
return null;
}
@@ -215,6 +245,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return boundingBoxResultMat;
}
/**
*
* @param target the target to find the corners of.
* @return the corners. left top, left bottom, right bottom, right top
*/
private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
// List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
@@ -300,8 +335,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
@Deprecated
private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
// Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);
// start by looking for corners
var points__ = findBoundingBoxCorners(target).toList();
var xList = points__.stream().map(it -> it.x).sorted(Double::compare).collect(Collectors.toList());