Skip to content

Commit

Permalink
Revert "Stream threading (#110)" (#111)
Browse files Browse the repository at this point in the history
This reverts commit 4c4b76a.
  • Loading branch information
mcm001 authored Sep 5, 2020
1 parent ef7c23c commit c1d2cbf
Show file tree
Hide file tree
Showing 10 changed files with 212 additions and 361 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@

package org.photonvision.vision.frame;

import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.opencv.core.Mat;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.Releasable;
Expand All @@ -37,12 +36,6 @@ public Frame(CVMat image, FrameStaticProperties frameStaticProperties) {
this(image, System.nanoTime(), frameStaticProperties);
}

public Frame() {
timestampNanos = 0;
image = new CVMat(new Mat());
frameStaticProperties = new FrameStaticProperties(0, 0, 0, new Rotation2d(), null);
}

public void copyTo(Frame destFrame) {
image.getMat().copyTo(destFrame.image.getMat());
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,8 @@
import org.opencv.core.Point;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.vision.opencv.ContourGroupingMode;
import org.photonvision.vision.opencv.ContourIntersectionDirection;
import org.photonvision.vision.opencv.ContourSortMode;
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.vision.target.RobotOffsetPointMode;
import org.photonvision.vision.target.TargetModel;
import org.photonvision.vision.target.TargetOffsetPointEdge;
import org.photonvision.vision.target.TargetOrientation;

Expand Down Expand Up @@ -75,60 +71,34 @@ public AdvancedPipelineSettings() {
public Point offsetDualPointB = new Point();
public double offsetDualPointBArea = 0;

// how many contours to attempt to group (Single, Dual)
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;

// the direction in which contours must intersect to be considered intersecting
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;

// 3d settings
public boolean solvePNPEnabled = false;
public TargetModel targetModel = TargetModel.get2020Target();

// Corner detection settings
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
public boolean cornerDetectionUseConvexHulls = true;
public boolean cornerDetectionExactSideCount = false;
public int cornerDetectionSideCount = 4;
public double cornerDetectionAccuracyPercentage = 10;

@Override
public boolean equals(Object o) {
if (this == o) return true;
if (!(o instanceof AdvancedPipelineSettings)) return false;
if (o == null || getClass() != o.getClass()) return false;
if (!super.equals(o)) return false;
AdvancedPipelineSettings that = (AdvancedPipelineSettings) o;
return outputShouldDraw == that.outputShouldDraw
&& outputShowMultipleTargets == that.outputShowMultipleTargets
&& erode == that.erode
&& dilate == that.dilate
&& contourSpecklePercentage == that.contourSpecklePercentage
&& Double.compare(that.offsetDualPointA.x, offsetDualPointA.x) == 0
&& Double.compare(that.offsetDualPointA.y, offsetDualPointA.y) == 0
&& Double.compare(that.offsetDualPointAArea, offsetDualPointAArea) == 0
&& Double.compare(that.offsetDualPointB.x, offsetDualPointB.x) == 0
&& Double.compare(that.offsetDualPointB.y, offsetDualPointB.y) == 0
&& Double.compare(that.offsetDualPointBArea, offsetDualPointBArea) == 0
&& solvePNPEnabled == that.solvePNPEnabled
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
&& cornerDetectionSideCount == that.cornerDetectionSideCount
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
== 0
&& Objects.equals(hsvHue, that.hsvHue)
&& Objects.equals(hsvSaturation, that.hsvSaturation)
&& Objects.equals(hsvValue, that.hsvValue)
&& Objects.equals(contourArea, that.contourArea)
&& Objects.equals(contourRatio, that.contourRatio)
&& Objects.equals(contourFullness, that.contourFullness)
&& hsvHue.equals(that.hsvHue)
&& hsvSaturation.equals(that.hsvSaturation)
&& hsvValue.equals(that.hsvValue)
&& contourArea.equals(that.contourArea)
&& contourRatio.equals(that.contourRatio)
&& contourFullness.equals(that.contourFullness)
&& contourSortMode == that.contourSortMode
&& contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge
&& contourTargetOrientation == that.contourTargetOrientation
&& offsetRobotOffsetMode == that.offsetRobotOffsetMode
&& Objects.equals(offsetSinglePoint, that.offsetSinglePoint)
&& Objects.equals(offsetDualPointA, that.offsetDualPointA)
&& Objects.equals(offsetDualPointB, that.offsetDualPointB)
&& contourGroupingMode == that.contourGroupingMode
&& contourIntersection == that.contourIntersection
&& Objects.equals(targetModel, that.targetModel)
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
&& offsetSinglePoint.equals(that.offsetSinglePoint);
}

@Override
Expand All @@ -154,15 +124,6 @@ public int hashCode() {
offsetDualPointA,
offsetDualPointAArea,
offsetDualPointB,
offsetDualPointBArea,
contourGroupingMode,
contourIntersection,
solvePNPEnabled,
targetModel,
cornerDetectionStrategy,
cornerDetectionUseConvexHulls,
cornerDetectionExactSideCount,
cornerDetectionSideCount,
cornerDetectionAccuracyPercentage);
offsetDualPointBArea);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -158,9 +158,7 @@ protected void setPipeParams(

var solvePNPParams =
new SolvePNPPipe.SolvePNPPipeParams(
frameStaticProperties.cameraCalibration,
frameStaticProperties.cameraPitch,
settings.targetModel);
settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
solvePNPPipe.setParams(solvePNPParams);

Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
Expand All @@ -180,12 +178,10 @@ protected void setPipeParams(
frameStaticProperties);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);

var draw3dTargetsParams =
var draw3dContoursParams =
new Draw3dTargetsPipe.Draw3dContoursParams(
settings.outputShouldDraw,
frameStaticProperties.cameraCalibration,
settings.targetModel);
draw3dTargetsPipe.setParams(draw3dTargetsParams);
settings.outputShouldDraw, settings.cameraCalibration, settings.targetModel);
draw3dTargetsPipe.setParams(draw3dContoursParams);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,14 @@
package org.photonvision.vision.pipeline;

import com.fasterxml.jackson.annotation.JsonTypeName;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.opencv.ContourGroupingMode;
import org.photonvision.vision.opencv.ContourIntersectionDirection;
import org.photonvision.vision.opencv.ContourShape;
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.vision.target.TargetModel;

@JsonTypeName("ColoredShapePipelineSettings")
public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
Expand All @@ -36,6 +42,25 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
public int minDist = 10;
public int maxCannyThresh = 90;
public int accuracy = 20;
// how many contours to attempt to group (Single, Dual)
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;

// the direction in which contours must intersect to be considered intersecting
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;

// 3d settings
public boolean solvePNPEnabled = false;
public CameraCalibrationCoefficients cameraCalibration;
public TargetModel targetModel;
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0); // TODO where should pitch live?

// Corner detection settings
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
public boolean cornerDetectionUseConvexHulls = true;
public boolean cornerDetectionExactSideCount = false;
public int cornerDetectionSideCount = 4;
public double cornerDetectionAccuracyPercentage = 10;

public ColoredShapePipelineSettings() {
super();
Expand All @@ -45,7 +70,7 @@ public ColoredShapePipelineSettings() {
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (!(o instanceof ColoredShapePipelineSettings)) return false;
if (o == null || getClass() != o.getClass()) return false;
if (!super.equals(o)) return false;
ColoredShapePipelineSettings that = (ColoredShapePipelineSettings) o;
return Double.compare(that.minArea, minArea) == 0
Expand All @@ -59,7 +84,19 @@ public boolean equals(Object o) {
&& minDist == that.minDist
&& maxCannyThresh == that.maxCannyThresh
&& accuracy == that.accuracy
&& desiredShape == that.desiredShape;
&& solvePNPEnabled == that.solvePNPEnabled
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
&& cornerDetectionSideCount == that.cornerDetectionSideCount
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
== 0
&& desiredShape == that.desiredShape
&& contourGroupingMode == that.contourGroupingMode
&& contourIntersection == that.contourIntersection
&& Objects.equals(cameraCalibration, that.cameraCalibration)
&& Objects.equals(targetModel, that.targetModel)
&& Objects.equals(cameraPitch, that.cameraPitch)
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
}

@Override
Expand All @@ -77,6 +114,17 @@ public int hashCode() {
maxRadius,
minDist,
maxCannyThresh,
accuracy);
accuracy,
contourGroupingMode,
contourIntersection,
solvePNPEnabled,
cameraCalibration,
targetModel,
cameraPitch,
cornerDetectionStrategy,
cornerDetectionUseConvexHulls,
cornerDetectionExactSideCount,
cornerDetectionSideCount,
cornerDetectionAccuracyPercentage);
}
}

This file was deleted.

Loading

0 comments on commit c1d2cbf

Please sign in to comment.