diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java index 64e09cc96..a612b9455 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java @@ -1,30 +1,39 @@ package com.chameleonvision.common.vision.opencv; +import com.chameleonvision.common.util.math.MathUtils; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.util.Comparator; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; +import org.opencv.imgproc.Moments; public class Contour { - private final MatOfPoint points; + public static final Comparator SortByMomentsX = + Comparator.comparingDouble( + (contour) -> contour.getMoments().get_m10() / contour.getMoments().get_m00()); + + public final MatOfPoint mat; private Double area = Double.NaN; private RotatedRect minAreaRect = null; private Rect boundingRect = null; + private Moments moments = null; - public Contour(MatOfPoint points) { - this.points = points; + public Contour(MatOfPoint mat) { + this.mat = mat; } public double getArea() { if (Double.isNaN(area)) { - area = Imgproc.contourArea(points); + area = Imgproc.contourArea(mat); } return area; } public RotatedRect getMinAreaRect() { if (minAreaRect == null) { - MatOfPoint2f temp = new MatOfPoint2f(points.toArray()); + MatOfPoint2f temp = new MatOfPoint2f(mat.toArray()); minAreaRect = Imgproc.minAreaRect(temp); temp.release(); } @@ -33,12 +42,67 @@ public class Contour { public Rect getBoundingRect() { if (boundingRect == null) { - boundingRect = Imgproc.boundingRect(points); + boundingRect = Imgproc.boundingRect(mat); } return boundingRect; } + public Moments getMoments() { + if (moments == null) { + moments = Imgproc.moments(mat); + } + return moments; + } + public Point getCenterPoint() { return getMinAreaRect().center; } + + public boolean isIntersecting( + Contour secondContour, TrackedTarget.TargetContourIntersection intersection) { + boolean isIntersecting = false; + + if (intersection == TrackedTarget.TargetContourIntersection.None) { + isIntersecting = true; + } else { + try { + MatOfPoint2f intersectMatA = new MatOfPoint2f(); + MatOfPoint2f intersectMatB = new MatOfPoint2f(); + intersectMatA.fromArray(mat.toArray()); + intersectMatB.fromArray(secondContour.mat.toArray()); + RotatedRect a = Imgproc.fitEllipse(intersectMatA); + RotatedRect b = Imgproc.fitEllipse(intersectMatB); + double mA = MathUtils.toSlope(a.angle); + double mB = MathUtils.toSlope(b.angle); + double x0A = a.center.x; + double y0A = a.center.y; + double x0B = b.center.x; + double y0B = b.center.y; + double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB); + double intersectionY = (mA * (intersectionX - x0A)) + y0A; + double massX = (x0A + x0B) / 2; + double massY = (y0A + y0B) / 2; + switch (intersection) { + case Up: + if (intersectionY < massY) isIntersecting = true; + break; + case Down: + if (intersectionY > massY) isIntersecting = true; + break; + case Left: + if (intersectionX < massX) isIntersecting = true; + break; + case Right: + if (intersectionX > massX) isIntersecting = true; + break; + } + intersectMatA.release(); + intersectMatB.release(); + } catch (Exception e) { + // defaults to false + } + } + + return isIntersecting; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java index 83c3ff435..734906398 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java @@ -1,112 +1,209 @@ package com.chameleonvision.common.vision.target; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.vision.opencv.Contour; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.apache.commons.math3.util.FastMath; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.RotatedRect; + // TODO: banks fix public class TrackedTarget { - // final Contour externalContour; - // - // private Point targetOffsetPoint = null; - // private Point robotOffsetPoint = null; - // - // // Single Grouped - // public TrackedTarget(Contour inputContour) { - // this.externalContour = inputContour; - // - // } - // - // public TrackedTarget(List subContours) { - // - // } - // - // - // private Point calculateOffsetPoint(boolean isLandscape, TargetOffsetPointRegion - // offsetRegion) { - // Point[] vertices = new Point[4]; - // - // RotatedRect minRect = externalContour.getMinAreaRect(); - // minRect.points(vertices); - // - // Point bl = getMiddle(vertices[0], vertices[1]); - // Point tl = getMiddle(vertices[1], vertices[2]); - // Point tr = getMiddle(vertices[2], vertices[3]); - // Point br = getMiddle(vertices[3], vertices[0]); - // boolean orientation; - // if (isLandscape) { - // orientation = minRect.size.width > minRect.size.height; - // } else { - // orientation = minRect.size.width < minRect.size.height; - // } - // - // Point result = minRect.center; - // switch (offsetRegion) { - // case Top: { - // result = orientation ? tl : tr; - // break; - // } - // case Bottom: { - // result = orientation ? br : bl; - // break; - // } - // case Left: { - // result = orientation ? bl : tl; - // break; - // } - // case Right: { - // result = orientation ? tr : br; - // break; - // } - // } - // return result; - // } - // - // public Point getTargetOffsetPoint(boolean isLandscape, TargetOffsetPointRegion offsetRegion) - // { - // if (targetOffsetPoint == null) { - // targetOffsetPoint = calculateOffsetPoint(isLandscape, offsetRegion); - // } - // return targetOffsetPoint; - // } - // - // private Point calculateRobotOffsetPoint(DoubleCouple offsetPoint, DoubleCouple - // offsetEquationValues, RobotOffsetPointMode offsetMode) { - // switch (offsetMode) { - // case Single: - // if (offsetPoint.isEmpty()) { - // offsetPoint.set(camProps.centerX, camProps.centerY); - // } - // - // t.calibratedX = offsetPoint.getFirst(); - // t.calibratedY = offsetPoint.getSecond(); - // break; - // case None: - // t.calibratedX = camProps.centerX; - // t.calibratedY = camProps.centerY; - // break; - // case Dual: - // t.calibratedX = (t.point.x - this.calibrationB) / this.calibrationM; - // t.calibratedY = (t.point.y * this.calibrationM) + this.calibrationB; - // break; - // } - // } - // - // private double calculatePitch(double pixelY, double centerY, double verticalFocalLength) { - // double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / - // verticalFocalLength)); - // return (pitch * -1); - // } - // - // private double calculateYaw(double pixelX, double centerX, double horizontalFocalLength) { - // return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / horizontalFocalLength)); - // } - // - // private Point getMiddle(Point p1, Point p2) { - // return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2)); - // } - // - // public enum TargetOffsetPointRegion { - // Center, Top, Bottom, Left, Right - // } - // - // public enum RobotOffsetPointMode { - // None, Single, Dual - // } + final Contour mainContour; + final List subContours = new ArrayList<>(); // can be empty + + private Point targetOffsetPoint = null; + private Point robotOffsetPoint = null; + + // Single Grouped + public TrackedTarget(Contour inputContour) { + mainContour = inputContour; + } + + // Dual grouping + public TrackedTarget( + List subContours, + TargetContourIntersection intersection, + TargetContourGrouping grouping) { + // do contour grouping + mainContour = calculateMultiContour(subContours, intersection, grouping); + if (mainContour == null) { + // this means we don't have a valid grouped target. what do we do??? + throw new RuntimeException("Something went fucky wucky"); + } + this.subContours.addAll(subContours); + } + + private Contour calculateMultiContour( + List input, TargetContourIntersection intersection, TargetContourGrouping grouping) { + int reqSize = grouping == TargetContourGrouping.Single ? 1 : 2; + + if (input.size() != reqSize) { + throw new RuntimeException("Insufficient contours for target grouping!"); + } + + switch (grouping) { + // technically should never happen but :shrug: + case Single: + return input.get(0); + case Dual: + input.sort(Contour.SortByMomentsX); + Collections.reverse(input); // why? + + Contour firstContour = input.get(0); + Contour secondContour = input.get(1); + + // total contour for both. add the first one for now + List fullContourPoints = new ArrayList<>(firstContour.mat.toList()); + + // add second contour if it is intersecting + if (firstContour.isIntersecting(secondContour, intersection)) { + fullContourPoints.addAll(secondContour.mat.toList()); + } else { + return null; + } + + MatOfPoint finalContour = new MatOfPoint(fullContourPoints.toArray(new Point[0])); + + if (finalContour.cols() != 0 && finalContour.rows() != 0) { + return new Contour(finalContour); + } + break; + } + return null; // + } + + private Point calculateOffsetPoint(boolean isLandscape, TargetOffsetPointRegion offsetRegion) { + Point[] vertices = new Point[4]; + + RotatedRect minRect = mainContour.getMinAreaRect(); + minRect.points(vertices); + + Point bl = getMiddle(vertices[0], vertices[1]); + Point tl = getMiddle(vertices[1], vertices[2]); + Point tr = getMiddle(vertices[2], vertices[3]); + Point br = getMiddle(vertices[3], vertices[0]); + boolean orientation; + if (isLandscape) { + orientation = minRect.size.width > minRect.size.height; + } else { + orientation = minRect.size.width < minRect.size.height; + } + + Point result = minRect.center; + switch (offsetRegion) { + case Top: + { + result = orientation ? tl : tr; + break; + } + case Bottom: + { + result = orientation ? br : bl; + break; + } + case Left: + { + result = orientation ? bl : tl; + break; + } + case Right: + { + result = orientation ? tr : br; + break; + } + } + return result; + } + + public Point getTargetOffsetPoint(boolean isLandscape, TargetOffsetPointRegion offsetRegion) { + if (targetOffsetPoint == null) { + targetOffsetPoint = calculateOffsetPoint(isLandscape, offsetRegion); + } + return targetOffsetPoint; + } + + private Point calculateRobotOffsetPoint( + Point offsetPoint, + Point camCenterPoint, + DoubleCouple offsetEquationValues, + RobotOffsetPointMode offsetMode) { + Point resultPoint = new Point(); + switch (offsetMode) { + case None: + resultPoint = camCenterPoint; + break; + case Single: + if (offsetPoint.x == 0 && offsetPoint.y == 0) { + resultPoint = camCenterPoint; + } else { + resultPoint = offsetPoint; + } + break; + case Dual: + resultPoint.x = + (offsetPoint.x - offsetEquationValues.getFirst()) / offsetEquationValues.getSecond(); + resultPoint.y = + (offsetPoint.y * offsetEquationValues.getSecond()) + offsetEquationValues.getFirst(); + break; + } + return resultPoint; + } + + public Point getRobotOffsetPoint( + Point userOffsetPoint, + Point camCenterPoint, + DoubleCouple offsetEquationValues, + RobotOffsetPointMode offsetMode) { + if (robotOffsetPoint == null) { + robotOffsetPoint = + calculateRobotOffsetPoint( + userOffsetPoint, camCenterPoint, offsetEquationValues, offsetMode); + } + return robotOffsetPoint; + } + + private double calculatePitch(double pixelY, double centerY, double verticalFocalLength) { + double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / verticalFocalLength)); + return (pitch * -1); + } + + private double calculateYaw(double pixelX, double centerX, double horizontalFocalLength) { + return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / horizontalFocalLength)); + } + + private Point getMiddle(Point p1, Point p2) { + return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2)); + } + + // TODO: move these? also docs plox + public enum TargetContourIntersection { + None, + Up, + Down, + Left, + Right + } + + public enum TargetContourGrouping { + Single, + Dual + } + + public enum TargetOffsetPointRegion { + Center, + Top, + Bottom, + Left, + Right + } + + public enum RobotOffsetPointMode { + None, + Single, + Dual + } }