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

V2.1.1 (#40)

Browse Source
This commit is contained in:
Matt
2020-01-08 00:11:34 -08:00
committed by oriagranat9
parent 97a6e1419d
commit 0e2e950d18
129 changed files with 2565 additions and 1170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
chameleon-server/src/main/java/com/chameleonvision/util/FileHelper.java
View File

@@ -32,13 +32,19 @@ public class FileHelper {
}
public static void setAllPerms(Path path) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);
p.waitFor();
if (!Platform.CurrentPlatform.isWindows()) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);
p.waitFor();
} catch (Exception e) {
e.printStackTrace();
} catch (Exception e) {
e.printStackTrace();
}
} else {
// TODO file perms on Windows
System.out.println("File permission setting not available on Windows. Not changing file permissions.");
}
}
}

3
chameleon-server/src/main/java/com/chameleonvision/vision/VisionManager.java
View File

@@ -44,7 +44,8 @@ public class VisionManager {
VideoCapture cap = new VideoCapture(info.dev);
if (cap.isOpened()) {
cap.release();
String name = info.name;
// Filter non-ascii characters because ext4 doesn't play nice with unicode in directory names
String name = info.name.replaceAll("[^\\x00-\\x7F]", "");
while (usbCameraInfosByCameraName.containsKey(name)) {
suffix++;
name = String.format("%s (%d)", name, suffix);

85
chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java
View File

@@ -24,7 +24,6 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpiutil.CircularBuffer;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
@@ -36,7 +35,7 @@ import java.util.stream.Collectors;
public class VisionProcess {
private final USBCameraCapture cameraCapture;
private final CameraStreamerRunnable streamRunnable;
// private final CameraStreamerRunnable streamRunnable;
private final VisionProcessRunnable visionRunnable;
private final CameraConfig fileConfig;
public final CameraStreamer cameraStreamer;
@@ -73,7 +72,7 @@ public class VisionProcess {
// Thread to put frames on the dashboard
this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
// this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
// Thread to process vision data
this.visionRunnable = new VisionProcessRunnable();
@@ -125,7 +124,7 @@ public class VisionProcess {
ntLatencyEntry = newTable.getEntry("latency");
ntValidEntry = newTable.getEntry("is_valid");
ntAuxListEntry = newTable.getEntry("aux_targets");
ntPoseEntry = newTable.getEntry("poseList");
ntPoseEntry = newTable.getEntry("pose");
ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
ntDriverModeEntry.setBoolean(false);
@@ -150,7 +149,11 @@ public class VisionProcess {
*/
private void setPipeline(EntryNotification notification) {
var wantedPipelineIndex = (int) notification.value.getDouble();
pipelineManager.setCurrentPipeline(wantedPipelineIndex);
if (pipelineManager.pipelines.size() - 1 < wantedPipelineIndex) {
ntPipelineEntry.setDouble(pipelineManager.getCurrentPipelineIndex());
} else {
pipelineManager.setCurrentPipeline(wantedPipelineIndex);
}
}
public void setDriverModeEntry(boolean isDriverMode) {
@@ -177,44 +180,49 @@ public class VisionProcess {
List<Double> center = new ArrayList<>();
if (data.hasTarget) {
if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data;
StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
for (var target : result.targets) {
pointMap = new HashMap<>();
pointMap.put("pitch", target.pitch);
pointMap.put("yaw", target.yaw);
pointMap.put("area", target.area);
pointMap.put("pose", target.cameraRelativePose);
try {
if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
for (var target : result.targets) {
pointMap = new HashMap<>();
pointMap.put("pitch", target.pitch);
pointMap.put("yaw", target.yaw);
pointMap.put("area", target.area);
pointMap.put("pose", target.cameraRelativePose);
webTargets.add(pointMap);
}
} else {
pointMap.put("pitch", bestTarget.pitch);
pointMap.put("yaw", bestTarget.yaw);
pointMap.put("area", bestTarget.area);
pointMap.put("pose", bestTarget.cameraRelativePose);
webTargets.add(pointMap);
}
} else {
pointMap.put("pitch", bestTarget.pitch);
pointMap.put("yaw", bestTarget.yaw);
pointMap.put("area", bestTarget.area);
pointMap.put("pose", bestTarget.cameraRelativePose);
webTargets.add(pointMap);
center.add(bestTarget.minAreaRect.center.x);
center.add(bestTarget.minAreaRect.center.y);
} catch (ClassCastException ignored) {
}
center.add(bestTarget.minAreaRect.center.x);
center.add(bestTarget.minAreaRect.center.y);
} else {
pointMap.put("pitch", null);
pointMap.put("yaw", null);
pointMap.put("area", null);
pointMap.put("pose", new Pose2d());
webTargets.add(pointMap);
center.add(null);
center.add(null);
}
} else {
pointMap.put("pitch", null);
pointMap.put("yaw", null);
pointMap.put("area", null);
pointMap.put("pose", new Pose2d());
webTargets.add(pointMap);
center.add(null);
center.add(null);
}
point.put("fps", visionRunnable.fps);
point.put("targets", webTargets);
point.put("rawPoint", center);
point.put("fps", visionRunnable.fps);
point.put("targets", webTargets);
point.put("rawPoint", center);
} else {
point.put("fps", visionRunnable.fps);
}
WebSend.put("point", point);
SocketHandler.broadcastMessage(WebSend);
}
@@ -233,12 +241,13 @@ public class VisionProcess {
ntYawEntry.setDouble(targets.get(0).yaw);
ntAreaEntry.setDouble(targets.get(0).area);
try {
Pose2d targetPose = targets.get(0).cameraRelativePose;
double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()};
ntPoseEntry.setDoubleArray(targetArray);
// ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose));
ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
.map(it -> List.of(it.pitch, it.yaw, it.area, it.cameraRelativePose))
.collect(Collectors.toList())));
// TODO: (2.1) 3d stuff...
ntPoseEntry.setString(objectMapper.writeValueAsString(targets.stream().map(target -> target.cameraRelativePose).collect(Collectors.toList())));
} catch (JsonProcessingException e) {
e.printStackTrace();
}
@@ -337,7 +346,7 @@ public class VisionProcess {
// streamFrameQueue.clear();
// streamFrameQueue.add(lastPipelineResult.outputMat);
var currentTime = System.currentTimeMillis();
if((currentTime - lastStreamTimeMs)/1000d > 1.0 / 30.0) {
if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) {
cameraStreamer.runStream(lastPipelineResult.outputMat);
// System.out.println("Ran stream in " + (System.currentTimeMillis() - currentTime) + "ms!");
lastStreamTimeMs = currentTime;

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
}

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java
View File

@@ -20,7 +20,6 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
private int checkerboardSquaresHigh = 7;
private int checkerboardSquaresWide = 7;
private MatOfPoint3f objP_ORIG;
private MatOfPoint3f objP;// new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3);
private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide);
@@ -45,11 +44,10 @@ public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverP
public Calibrate3dPipeline(StandardCVPipelineSettings settings) {
super(settings);
objP_ORIG = new MatOfPoint3f();
objP = new MatOfPoint3f();
for(int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) {
objP_ORIG.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
objP.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
}
setSquareSize(checkerboardSquareSizeUnits);

37
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,14 +192,14 @@ 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(collect2dTargetsResult.getLeft());
Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), hsvResult.getLeft()));
totalPipelineTimeNanos += solvePNPResult.getRight();
Pair<Mat, Long> draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
@@ -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
@@ -263,12 +268,16 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
public Pair<Rect, Rect> leftRightDualTargetPair = null;
public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
public MatOfPoint2f rawContour = kMat2f;
public MatOfPoint2f approxPoly = new MatOfPoint2f();
public void release() {
rVector.release();
tVector.release();
imageCornerPoints.release();
}
private static final MatOfPoint2f kMat2f = new MatOfPoint2f();
}

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();
}

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

@@ -17,11 +17,14 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
public Scalar color = Helpers.colorToScalar(Color.GREEN);
public Scalar green = Helpers.colorToScalar(Color.GREEN);
public Scalar blue = Helpers.colorToScalar(Color.BLUE);
public Scalar red = Helpers.colorToScalar(Color.RED);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
setObjectBox(14.5, 6, 2);
// setObjectBox(14.5, 6, 2);
set2020Box();
}
public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
@@ -40,6 +43,20 @@ 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(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0),
new Point3(-16.25, 0, -6),
new Point3(-9.819867, -17, -6),
new Point3(9.819867, -17, -6),
new Point3(16.25, 0, -6)
);
}
private Mat cameraMatrix = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
@@ -56,13 +73,15 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
this.distortionCoefficients = distortionMatrix_;
}
MatOfPoint2f imagePoints = new MatOfPoint2f();
@Override
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> targets) {
long processStartNanos = System.nanoTime();
var image = targets.getLeft();
for(var it : targets.getRight()) {
MatOfPoint2f imagePoints = new MatOfPoint2f();
try {
Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
} catch (Exception e) {
@@ -81,26 +100,33 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
// 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, new Scalar(0, 255, 0), 5);
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);
}
// sketch out floor
Imgproc.line(image, pts.get(0), pts.get(1), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(2), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(3), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(0), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(0), pts.get(1), green, 3);
Imgproc.line(image, pts.get(1), pts.get(2), green, 3);
Imgproc.line(image, pts.get(2), pts.get(3), green, 3);
Imgproc.line(image, pts.get(3), pts.get(0), green, 3);
// draw pillars
Imgproc.line(image, pts.get(0), pts.get(4), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(5), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(6), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(7), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(0), pts.get(4), blue, 3);
Imgproc.line(image, pts.get(1), pts.get(5), blue, 3);
Imgproc.line(image, pts.get(2), pts.get(6), blue, 3);
Imgproc.line(image, pts.get(3), pts.get(7), blue, 3);
// draw top
Imgproc.line(image, pts.get(4), pts.get(5), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(5), pts.get(6), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(6), pts.get(7), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(7), pts.get(4), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(4), pts.get(5), red, 3);
Imgproc.line(image, pts.get(5), pts.get(6), red, 3);
Imgproc.line(image, pts.get(6), pts.get(7), red, 3);
Imgproc.line(image, pts.get(7), pts.get(4), red, 3);
}
long processTime = System.nanoTime() - processStartNanos;

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

@@ -23,6 +23,8 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
private TargetGroup group;
private TargetIntersection intersection;
private MatOfPoint2f contourBuffer = new MatOfPoint2f();
private List<StandardCVPipeline.TrackedTarget> groupedContours = new ArrayList<>();
private MatOfPoint2f intersectMatA = new MatOfPoint2f();
private MatOfPoint2f intersectMatB = new MatOfPoint2f();
@@ -43,6 +45,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
groupedContours.forEach(StandardCVPipeline.TrackedTarget::release);
groupedContours.clear();
contourBuffer.release();
if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
@@ -54,12 +57,12 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
switch (group) {
case Single: {
input.forEach(c -> {
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromArray(c.toArray());
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
contourBuffer.fromArray(c.toArray());
if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.rawContour = contourBuffer;
groupedContours.add(target);
}
});
@@ -83,26 +86,28 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
intersectMatA.release();
intersectMatB.release();
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromList(finalContourList);
contourBuffer.fromList(finalContourList);
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
// find left and right bouding rectangles
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),
Imgproc.boundingRect(secondContour));
// find left and right min area rectangles
tempRectMat.fromArray(firstContour.toArray());
var minAreaRect1 = Imgproc.minAreaRect(tempRectMat);
tempRectMat.fromArray(secondContour.toArray());
var minAreaRect2 = Imgproc.minAreaRect(tempRectMat);
target.leftRightRotatedRect =
Pair.of(minAreaRect1, minAreaRect2);
target.rawContour = contourBuffer;
groupedContours.add(target);
firstContour.release();

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

@@ -13,14 +13,10 @@ import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.*;
import java.util.stream.Collectors;
public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, Mat>, List<StandardCVPipeline.TrackedTarget>> {
private Double tilt_angle;
private MatOfPoint3f objPointsMat = new MatOfPoint3f();
@@ -41,11 +37,28 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
super();
setCameraCoeffs(calibration);
setTarget(settings.targetWidth, settings.targetHeight);
// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
// TODO add proper year differentiation
set2020Target(true);
this.tilt_angle = tilt.getRadians();
}
public void setTarget(double targetWidth, double targetHeight) {
public void set2020Target(boolean isHighGoal) {
if(isHighGoal) {
// tl, bl, br, tr is the order
List<Point3> corners = List.of(
new Point3(-16.25, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0));
setObjectCorners(corners);
} else {
setBoundingBoxTarget(7, 11);
}
}
public void setBoundingBoxTarget(double targetWidth, double targetHeight) {
// order is left top, left bottom, right bottom, right top
List<Point3> corners = List.of(
@@ -65,7 +78,8 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) {
setCameraCoeffs(camConfig);
setTarget(settings.targetWidth, settings.targetHeight);
// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
// TODO add proper year differentiation
tilt_angle = tilt.getRadians();
}
@@ -86,11 +100,13 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
}
@Override
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> targets) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, Mat> imageTargetPair) {
long processStartNanos = System.nanoTime();
var targets = imageTargetPair.getLeft();
poseList.clear();
for(var target: targets) {
var corners = (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);
}
@@ -121,10 +137,54 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
return points;
}
private MatOfPoint2f findCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) {
/**
* Find the target using the outermost tape corners and a 2020 target.
* @param target the target.
* @return The four outermost tape corners.
*/
private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target) {
if(target.rawContour.cols() < 1) return null;
var centroid = target.minAreaRect.center;
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 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
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));
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.
* @return The four outermost tape corners.
*/
private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) {
if(target.leftRightRotatedRect == null) return null;
var centroid = target.minAreaRect.center;
Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
var left = target.leftRightRotatedRect.getLeft();
var right = target.leftRightRotatedRect.getRight();
@@ -144,8 +204,6 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
combinedList.addAll(List.of(rightPoints));
// start looking in the top left quadrant
Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
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();
@@ -244,13 +302,19 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
// 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),
@@ -265,10 +329,10 @@ public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>
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<Point> 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 ->