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

Target region (#38)

Browse Source 
* WIP: target region

* working math

* working directions

* removed if statements

* working portrait and landscape

* Revert "working portrait and landscape "

This reverts commit 8ce2ead1b41d372f72128b7fceeaab09e0834eaa.

* finalized targeting region
This commit is contained in:
oriagranat9
2020-01-07 13:07:52 -08:00
committed by GitHub
parent 37a1c60595
commit b3b64d6d89
10 changed files with 2300 additions and 918 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
chameleon-server/src/main/java/com/chameleonvision/vision/enums/TargetOrientation.java Normal file
View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision.enums;
public enum TargetOrientation {
Portrait, Landscape
}

5
chameleon-server/src/main/java/com/chameleonvision/vision/enums/TargetRegion.java Normal file
View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision.enums;
public enum TargetRegion {
Center, Top, Bottom, Left, Right
}

31
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
View File

@@ -10,7 +10,9 @@ import com.chameleonvision.vision.pipeline.pipes.*;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.core.Point;
import java.awt.*;
import java.util.List;
import static com.chameleonvision.vision.pipeline.impl.StandardCVPipeline.*;
@@ -71,19 +73,19 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue());
groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection);
sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps, 5);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB, camProps);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.targetRegion, settings.targetOrientation, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
draw2dCrosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
// TODO: make settable from UI? config?
draw2dContoursSettings.showCentroid = false;
draw2dContoursSettings.showCentroid = true;
draw2dContoursSettings.centroidColor = new Color(25, 239, 0);
draw2dContoursSettings.boxOutlineSize = 2;
draw2dContoursSettings.showRotatedBox = true;
draw2dContoursSettings.showMaximumBox = true;
draw2dContoursSettings.showMultiple = settings.multiple;
draw2dCrosshairPipeSettings.showCrosshair=true;
draw2dCrosshairPipeSettings.showCrosshair = true;
draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
draw2dCrosshairPipe=new Draw2dCrosshairPipe(draw2dCrosshairPipeSettings,settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB);
draw2dCrosshairPipe = new Draw2dCrosshairPipe(draw2dCrosshairPipeSettings, settings.calibrationMode, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB);
outputMatPipe = new OutputMatPipe(settings.isBinary);
}
@@ -124,14 +126,16 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
speckleRejectPipe.setConfig(settings.speckle.doubleValue());
groupContoursPipe.setConfig(settings.targetGroup, settings.targetIntersection);
sortContoursPipe.setConfig(settings.sortMode, camProps, 5);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB, camProps);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.targetRegion, settings.targetOrientation, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
draw2dContoursPipe.setConfig(settings.multiple, camProps);
draw2dCrosshairPipe.setConfig(draw2dCrosshairPipeSettings,settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB);
draw2dCrosshairPipe.setConfig(draw2dCrosshairPipeSettings, settings.calibrationMode, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB);
outputMatPipe.setConfig(settings.isBinary);
if(settings.is3D) {
if(solvePNPPipe == null) solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
if(drawSolvePNPPipe == null) drawSolvePNPPipe = new DrawSolvePNPPipe(cameraCapture.getCurrentCalibrationData());
if (settings.is3D) {
if (solvePNPPipe == null)
solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
if (drawSolvePNPPipe == null)
drawSolvePNPPipe = new DrawSolvePNPPipe(cameraCapture.getCurrentCalibrationData());
solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData());
@@ -179,7 +183,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
Pair<Mat, Long> result;
if(!settings.is3D) {
if (!settings.is3D) {
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
result = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
totalPipelineTimeNanos += result.getRight();
@@ -188,12 +192,12 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
}
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
Pair<Mat, Long> draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(result.getLeft(),collect2dTargetsResult.getLeft()));
Pair<Mat, Long> draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(result.getLeft(), collect2dTargetsResult.getLeft()));
totalPipelineTimeNanos += draw2dCrosshairResult.getRight();
Mat outputMat;
if(settings.is3D) {
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(Pair.of(collect2dTargetsResult.getLeft(), hsvResult.getLeft()));
totalPipelineTimeNanos += solvePNPResult.getRight();
@@ -254,6 +258,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public double pitch = 0.0;
public double yaw = 0.0;
public double area = 0.0;
public Point point = new Point();
public RotatedRect minAreaRect;
// 3d stuff

7
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java
View File

@@ -1,9 +1,6 @@
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.vision.enums.CalibrationMode;
import com.chameleonvision.vision.enums.SortMode;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import com.chameleonvision.vision.enums.*;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.fasterxml.jackson.annotation.JsonIgnore;
import edu.wpi.first.wpilibj.util.Units;
@@ -27,6 +24,8 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
public Number speckle = 5;
public boolean isBinary = false;
public SortMode sortMode = SortMode.Largest;
public TargetRegion targetRegion = TargetRegion.Center;
public TargetOrientation targetOrientation = TargetOrientation.Landscape;
public boolean multiple = false;
public TargetGroup targetGroup = TargetGroup.Single;
public TargetIntersection targetIntersection = TargetIntersection.Up;

65
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Collect2dTargetsPipe.java
View File

@@ -1,14 +1,16 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.enums.TargetOrientation;
import com.chameleonvision.vision.enums.TargetRegion;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.enums.CalibrationMode;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Point;
import java.util.ArrayList;
import java.util.List;
import java.util.*;
public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties>, List<StandardCVPipeline.TrackedTarget>> {
@@ -17,18 +19,23 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
private CalibrationMode calibrationMode;
private List<Number> calibrationPoint;
private double calibrationM, calibrationB;
private TargetRegion targetRegion;
private TargetOrientation targetOrientation;
private List<StandardCVPipeline.TrackedTarget> targets = new ArrayList<>();
private Point[] vertices = new Point[4];
public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
setConfig(calibrationMode, calibrationPoint, calibrationM, calibrationB, camProps);
public Collect2dTargetsPipe(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
setConfig(calibrationMode, targetRegion, targetOrientation, calibrationPoint, calibrationM, calibrationB, camProps);
}
public void setConfig(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
public void setConfig(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
this.calibrationMode = calibrationMode;
this.calibrationPoint = calibrationPoint;
this.calibrationM = calibrationM;
this.calibrationB = calibrationB;
this.camProps = camProps;
this.targetRegion = targetRegion;
this.targetOrientation = targetOrientation;
}
@Override
@@ -41,10 +48,42 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
if (input.size() > 0) {
for (var t : input) {
t.minAreaRect.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 (targetOrientation == TargetOrientation.Landscape) {
orientation = t.minAreaRect.size.width > t.minAreaRect.size.height;
} else {
orientation = t.minAreaRect.size.width < t.minAreaRect.size.height;
}
Point result = t.minAreaRect.center;
switch (this.targetRegion) {
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;
}
}
t.point = result;
switch (this.calibrationMode) {
case Single:
if(this.calibrationPoint.isEmpty())
{
if (this.calibrationPoint.isEmpty()) {
this.calibrationPoint.add(camProps.centerX);
this.calibrationPoint.add(camProps.centerY);
}
@@ -56,13 +95,13 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
t.calibratedY = camProps.centerY;
break;
case Dual:
t.calibratedX = (t.minAreaRect.center.y - this.calibrationB) / this.calibrationM;
t.calibratedY = (t.minAreaRect.center.x * this.calibrationM) + this.calibrationB;
t.calibratedX = (t.point.x - this.calibrationB) / this.calibrationM;
t.calibratedY = (t.point.y * this.calibrationM) + this.calibrationB;
break;
}
t.pitch = calculatePitch(t.minAreaRect.center.y, t.calibratedY);
t.yaw = calculateYaw(t.minAreaRect.center.x, t.calibratedX);
t.pitch = calculatePitch(t.point.y, t.calibratedY);
t.yaw = calculateYaw(t.point.x, t.calibratedX);
t.area = t.minAreaRect.size.area() / imageArea;
targets.add(t);
@@ -81,4 +120,8 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.T
private double calculateYaw(double pixelX, double centerX) {
return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / camProps.horizontalFocalLength));
}
private Point getMiddle(Point p1, Point p2) {
return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2));
}
}

7
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dContoursPipe.java
View File

@@ -64,9 +64,6 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipelin
// MatOfPoint contour = new MatOfPoint(vertices);
drawnContours.add(contour);
if (settings.showCentroid) {
Imgproc.circle(input.getLeft(), r.center, 3, Helpers.colorToScalar(settings.centroidColor));
}
if (settings.showRotatedBox) {
Imgproc.drawContours(input.getLeft(), drawnContours, 0, Helpers.colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize);
@@ -76,6 +73,10 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipelin
Rect box = Imgproc.boundingRect(contour);
Imgproc.rectangle(input.getLeft(), new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), Helpers.colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize);
}
if (settings.showCentroid) {
Imgproc.circle(input.getLeft(), target.point, 3, Helpers.colorToScalar(settings.centroidColor), 2);
}
// contour.release();
}