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

Use approx poly for 2020 solvePNP

Browse Source
This commit is contained in:
Matt
2020-01-05 21:56:14 -08:00
parent 155ad6f70a
commit 46e66b9a3b
4 changed files with 75 additions and 50 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

@@ -195,7 +195,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
if(settings.is3D) {
// once we've sorted our targets, perform solvePNP. The number of "best targets" is limited by the above pipe
Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(collect2dTargetsResult.getLeft());
Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), hsvResult.getLeft()));
totalPipelineTimeNanos += solvePNPResult.getRight();
Pair<Mat, Long> draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
@@ -264,6 +264,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public Pair<Rect, Rect> leftRightDualTargetPair = null;
public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
public MatOfPoint2f rawContour = kMat2f;
public MatOfPoint2f approxPoly = new MatOfPoint2f();
public void release() {
rVector.release();

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

@@ -17,7 +17,9 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
public Scalar color = Helpers.colorToScalar(Color.GREEN);
public Scalar green = Helpers.colorToScalar(Color.GREEN);
public Scalar blue = Helpers.colorToScalar(Color.BLUE);
public Scalar red = Helpers.colorToScalar(Color.RED);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
@@ -81,27 +83,33 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
// 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, new Scalar(0, 255, 0), 5);
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.circle(image, it.minAreaRect.center, 4, new Scalar(200, 0, 100), 5);
// sketch out floor
Imgproc.line(image, pts.get(0), pts.get(1), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(2), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(3), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(0), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(0), pts.get(1), green, 3);
Imgproc.line(image, pts.get(1), pts.get(2), green, 3);
Imgproc.line(image, pts.get(2), pts.get(3), green, 3);
Imgproc.line(image, pts.get(3), pts.get(0), green, 3);
// draw pillars
Imgproc.line(image, pts.get(0), pts.get(4), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(5), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(6), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(7), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(0), pts.get(4), blue, 3);
Imgproc.line(image, pts.get(1), pts.get(5), blue, 3);
Imgproc.line(image, pts.get(2), pts.get(6), blue, 3);
Imgproc.line(image, pts.get(3), pts.get(7), blue, 3);
// draw top
Imgproc.line(image, pts.get(4), pts.get(5), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(5), pts.get(6), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(6), pts.get(7), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(7), pts.get(4), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(4), pts.get(5), red, 3);
Imgproc.line(image, pts.get(5), pts.get(6), red, 3);
Imgproc.line(image, pts.get(6), pts.get(7), red, 3);
Imgproc.line(image, pts.get(7), pts.get(4), red, 3);
}
long processTime = System.nanoTime() - processStartNanos;

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

@@ -13,13 +13,10 @@ import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.*;
import java.util.stream.Collectors;
public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>, List<StandardCVPipeline.TrackedTarget>> {
private Double tilt_angle;
private MatOfPoint3f objPointsMat = new MatOfPoint3f();
@@ -103,11 +100,12 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
}
@Override
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> targets) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, Mat> imageTargetPair) {
long processStartNanos = System.nanoTime();
var targets = imageTargetPair.getLeft();
poseList.clear();
for(var target: targets) {
var corners = find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
var corners = find2020VisionTarget(target);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
if(corners == null) continue;
var pose = calculatePose(corners, target);
if(pose != null) poseList.add(pose);
@@ -153,18 +151,30 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
var contour = target.rawContour;
var combinedList = contour.toList();
// approx poly dp time
Imgproc.approxPolyDP(contour, polyOutput, 3, true);
var polyList = polyOutput.toList();
polyOutput.copyTo(target.approxPoly);
// 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();
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();
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));
boundingBoxResultMat.release();
boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
return boundingBoxResultMat;
return boundingBoxResultMat;
} catch (NoSuchElementException e) {
return null;
}
}
MatOfPoint2f polyOutput = new MatOfPoint2f();
/**
* Find the target using the outermost tape corners and a dual target.
* @param target the target.
@@ -292,13 +302,19 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
// Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);
// start by looking at the targets
var leftRight = target.leftRightDualTargetPair;
var reverse = (leftRight.getLeft().x < leftRight.getRight().x);
var left = reverse ? leftRight.getLeft() : leftRight.getRight();
var right = !reverse ? leftRight.getLeft() : leftRight.getRight();
var boundingTl = left.tl();
var boundingBr = right.br();
// start by looking for corners
var points__ = findBoundingBoxCorners(target).toList();
var xList = points__.stream().map(it -> it.x).sorted(Double::compare).collect(Collectors.toList());
var yList = points__.stream().map(it -> it.y).sorted(Double::compare).collect(Collectors.toList());
var boundingTl = new Point(
xList.get(0), yList.get(0)
);
var boundingBr = new Point (
xList.get(2), yList.get(2)
);
System.out.println("tl/br:\n" + boundingTl.toString() + "\n" + boundingBr.toString());
var slightlyBiggerTl = new Point(
Math.max(0, boundingTl.x - 5),
@@ -313,10 +329,10 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
var croppedImage = srcImage.submat(rect);
var corners = new MatOfPoint();
Imgproc.goodFeaturesToTrack(croppedImage, corners, 8,0.5,5);
Imgproc.goodFeaturesToTrack(croppedImage, corners, 0,0.01,5);
List<Point> cornerList = new ArrayList<>(corners.toList());
if(cornerList.size() != 8 && cornerList.size() != 4) return null;
// if(cornerList.size() != 8 && cornerList.size() != 4) return null;
cornerList.sort(leftRightComparator);
cornerList = cornerList.stream().map(point ->