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

2.2 (#52)

Browse Source 
## features
-- added pi cam support (from wpilib v2020.2.2)
-- added gain slider for ps3 eye
-- added custom pnp model upload
-- network tables will try to reconnect if no connection
-- re did network tables key and added new values 


## bug fixes
-- fixed solve pnp not detection
-- lowered minimum fps for some camera
-- fixed vision hang bug
This commit is contained in:
Banks T
2020-01-22 14:53:11 -05:00
committed by oriagranat9
parent 0e2e950d18
commit a0b89168b4
30 changed files with 719 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/PipelineManager.java
View File

@@ -118,7 +118,8 @@ public class PipelineManager {
}
public void setCurrentPipeline(int index) {
CVPipeline newPipeline=null;
CVPipeline newPipeline = null;
if (index == DRIVERMODE_INDEX) {
newPipeline = driverModePipeline;
@@ -165,6 +166,11 @@ public class PipelineManager {
ntIndexEntry.setDouble(index);
}
}
// gain setting quirk
if (!parentProcess.cameraCapture.getProperties().isPS3Eye) {
getCurrentPipeline().settings.gain = -1;
}
}
public void addPipeline(CVPipelineSettings setting) {

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

@@ -235,7 +235,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
System.out.printf("full pipeline run time was %.3fms (%.2fFPS)\n", truePipelineTimeMillis, truePipelineFPS);
}
memManager.run();
// memManager.run();
resultCache = new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos);
return resultCache;
@@ -260,6 +260,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public double area = 0.0;
public Point point = new Point();
public RotatedRect minAreaRect;
public Rect boundingRect;
// 3d stuff
public Pose2d cameraRelativePose = new Pose2d();

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

@@ -4,10 +4,7 @@ import com.chameleonvision.vision.enums.*;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.fasterxml.jackson.annotation.JsonIgnore;
import edu.wpi.first.wpilibj.util.Units;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.*;
import java.util.Arrays;
import java.util.List;
@@ -20,7 +17,7 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
public boolean dilate = false;
public List<Number> area = Arrays.asList(0.0, 100.0);
public List<Number> ratio = Arrays.asList(0.0, 20.0);
public List<Number> extent = Arrays.asList(0, 100);
public List<Number> extent = Arrays.asList(0.0, 100.0);
public Number speckle = 5;
public boolean isBinary = false;
public SortMode sortMode = SortMode.Largest;
@@ -35,7 +32,22 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
public double dualTargetCalibrationB = 0;
// 3d stuff
public double targetWidth = 15.5, targetHeight = 6.0;
public MatOfPoint3f targetCornerMat = new MatOfPoint3f();
private static MatOfPoint3f hexTargetMat = new MatOfPoint3f();
static {
hexTargetMat.fromList(List.of(
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(19.625, 0, 0)));
}
public StandardCVPipelineSettings() {
super();
hexTargetMat.copyTo(targetCornerMat);
}
public boolean is3D = false;
}

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

@@ -16,10 +16,12 @@ import java.util.List;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
private MatOfPoint tempMatOfPoints = new MatOfPoint();
public Scalar green = Helpers.colorToScalar(Color.GREEN);
public Scalar blue = Helpers.colorToScalar(Color.BLUE);
public Scalar red = Helpers.colorToScalar(Color.RED);
public Scalar orange = Helpers.colorToScalar(Color.orange);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
@@ -46,14 +48,14 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
public void set2020Box() {
boxCornerMat.release();
boxCornerMat = new MatOfPoint3f(
new Point3(-16.25, 0, 0),
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0),
new Point3(-16.25, 0, -6),
new Point3(19.625, 0, 0),
new Point3(-19.625, 0, -6),
new Point3(-9.819867, -17, -6),
new Point3(9.819867, -17, -6),
new Point3(16.25, 0, -6)
new Point3(19.625, 0, -6)
);
}
@@ -97,17 +99,20 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2);
}
// 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, 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.line(image, list.get(i), next, red, 2);
}
// draw center
Imgproc.circle(image, it.minAreaRect.center, 5, red);
// 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, green, 5);
}
// sketch out floor

6
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
View File

@@ -7,6 +7,7 @@ import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
@@ -60,9 +61,11 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
contourBuffer.fromArray(c.toArray());
if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
Rect boundingRect = Imgproc.boundingRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.rawContour = contourBuffer;
target.boundingRect = boundingRect;
groupedContours.add(target);
}
});
@@ -90,9 +93,10 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
Rect boundingRect = Imgproc.boundingRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.boundingRect = boundingRect;
// find left and right bouding rectangles
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),

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

@@ -48,10 +48,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
if(isHighGoal) {
// tl, bl, br, tr is the order
List<Point3> corners = List.of(
new Point3(-16.25, 0, 0),
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0));
new Point3(19.625, 0, 0));
setObjectCorners(corners);
} else {
setBoundingBoxTarget(7, 11);
@@ -81,6 +82,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
// TODO add proper year differentiation
tilt_angle = tilt.getRadians();
this.objPointsMat = settings.targetCornerMat;
}
private void setCameraCoeffs(CameraCalibrationConfig settings) {
@@ -137,6 +139,16 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return points;
}
MatOfPoint2f target2020ResultMat = new MatOfPoint2f();
private double distanceBetween(Point a, Point b) {
return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
}
MatOfInt tempInt = new MatOfInt();
MatOfPoint2f tempMat2f = new MatOfPoint2f();
MatOfPoint tempMatOfPoint = new MatOfPoint();
/**
* Find the target using the outermost tape corners and a 2020 target.
* @param target the target.
@@ -149,25 +161,46 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
var contour = target.rawContour;
var convex = tempInt;
tempMatOfPoint.fromList(contour.toList());
Imgproc.convexHull(tempMatOfPoint, convex);
var combinedList = contour.toList();
// approx poly dp time
Imgproc.approxPolyDP(contour, polyOutput, 3, true);
Point[] contourArray = contour.toArray();
Point[] hullPoints = new Point[convex.rows()];
List<Integer> hullContourIdxList = convex.toList();
for (int i = 0; i < hullContourIdxList.size(); i++) {
hullPoints[i] = contourArray[hullContourIdxList.get(i)];
}
tempMat2f.fromArray(hullPoints);
Imgproc.approxPolyDP(tempMat2f, polyOutput, 5, true);
var polyList = polyOutput.toList();
polyOutput.copyTo(target.approxPoly);
// start looking in the top left quadrant
// left top, left bottom, right bottom, right top
var boundingBoxCorners = findBoundingBoxCorners(target).toList();
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();
// top left and top right are the poly corners closest to the bouding box tl and tr
Point tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
Point tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get();
// 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));
target2020ResultMat.release();
target2020ResultMat.fromList(List.of(tl, bl, br, tr));
return boundingBoxResultMat;
return target2020ResultMat;
} catch (NoSuchElementException e) {
return null;
}
@@ -215,6 +248,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return boundingBoxResultMat;
}
/**
*
* @param target the target to find the corners of.
* @return the corners. left top, left bottom, right bottom, right top
*/
private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
// List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
@@ -300,8 +338,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
@Deprecated
private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
// Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);
// start by looking for corners
var points__ = findBoundingBoxCorners(target).toList();
var xList = points__.stream().map(it -> it.x).sorted(Double::compare).collect(Collectors.toList());
@@ -313,7 +349,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
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(