diff --git a/chameleon-server/chameleon-vision.iml b/chameleon-server/chameleon-vision.iml
index 0b98abedd..313fedcae 100644
--- a/chameleon-server/chameleon-vision.iml
+++ b/chameleon-server/chameleon-vision.iml
@@ -58,16 +58,16 @@
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
index 9f082e921..84c0863b5 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
@@ -195,7 +195,7 @@ public class StandardCVPipeline extends CVPipeline, Long> solvePNPResult = solvePNPPipe.run(collect2dTargetsResult.getLeft());
+ Pair, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), hsvResult.getLeft()));
totalPipelineTimeNanos += solvePNPResult.getRight();
Pair draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
@@ -264,6 +264,7 @@ public class StandardCVPipeline extends CVPipeline leftRightDualTargetPair = null;
public Pair leftRightRotatedRect = null;
public MatOfPoint2f rawContour = kMat2f;
+ public MatOfPoint2f approxPoly = new MatOfPoint2f();
public void release() {
rVector.release();
diff --git a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
index da724ecbd..563553f2e 100644
--- a/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
+++ b/chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java
@@ -17,7 +17,9 @@ public class DrawSolvePNPPipe implements Pipe, List> {
+public class SolvePNPPipe implements Pipe, Mat>, List> {
private Double tilt_angle;
private MatOfPoint3f objPointsMat = new MatOfPoint3f();
@@ -103,11 +100,12 @@ public class SolvePNPPipe implements Pipe
}
@Override
- public Pair, Long> run(List targets) {
+ public Pair, Long> run(Pair, 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
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
// 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
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 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 ->