Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-01 02:41:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Target grouping now in TrackedTarget

Browse Source
This commit is contained in:
Banks Troutman
2020-03-29 03:19:10 -04:00
parent 90d463a530
commit 7f389c5c9e
2 changed files with 274 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java
View File

@@ -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<Contour> 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;
}
}

311
chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java
View File

@@ -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<Contour> 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<Contour> 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<Contour> 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<Contour> 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<Point> 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
}
}