Skip to content
Merged
Show file tree
Hide file tree
Changes from 21 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions photon-client/src/components/dashboard/tabs/OutputTab.vue
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ import PvSelect from "@/components/common/pv-select.vue";
import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
import { type ActivePipelineSettings, PipelineType, RobotOffsetPointMode } from "@/types/PipelineTypes";
import PvSwitch from "@/components/common/pv-switch.vue";
import PvSlider from "@/components/common/pv-slider.vue";
import { computed } from "vue";
import { RobotOffsetType } from "@/types/SettingTypes";
import { useStateStore } from "@/stores/StateStore";
Expand Down Expand Up @@ -58,14 +59,17 @@ const interactiveCols = computed(() =>

<template>
<div>
<pv-switch
v-model="useCameraSettingsStore().currentPipelineSettings.outputShowMultipleTargets"
label="Show Multiple Targets"
tooltip="If enabled, up to five targets will be displayed and sent via PhotonLib, instead of just one"
<pv-slider
v-model="useCameraSettingsStore().currentPipelineSettings.outputMaximumTargets"
label="Maximum Targets"
tooltip="The maximum number of targets to display and send."
:disabled="isTagPipeline"
:min="1"
:max="127"
:step="1"
:switch-cols="interactiveCols"
@update:modelValue="
(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ outputShowMultipleTargets: value }, false)
(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ outputMaximumTargets: value }, false)
"
/>
<pv-switch
Expand Down
19 changes: 9 additions & 10 deletions photon-client/src/types/PipelineTypes.ts
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ export interface PipelineSettings {
hsvHue: WebsocketNumberPair | [number, number];
ledMode: boolean;
hueInverted: boolean;
outputShowMultipleTargets: boolean;
outputMaximumTargets: number;
contourSortMode: number;
cameraExposureRaw: number;
cameraMinExposureRaw: number;
Expand Down Expand Up @@ -108,7 +108,7 @@ export type ConfigurablePipelineSettings = Partial<
// Omitted settings are changed for all pipeline types
export const DefaultPipelineSettings: Omit<
PipelineSettings,
"cameraGain" | "targetModel" | "ledMode" | "outputShowMultipleTargets" | "cameraExposureRaw" | "pipelineType"
"cameraGain" | "targetModel" | "ledMode" | "cameraExposureRaw" | "pipelineType"
> = {
offsetRobotOffsetMode: RobotOffsetPointMode.None,
streamingFrameDivisor: 0,
Expand Down Expand Up @@ -137,6 +137,7 @@ export const DefaultPipelineSettings: Omit<
offsetDualPointB: { x: 0, y: 0 },
hsvHue: { first: 50, second: 180 },
hueInverted: false,
outputMaximumTargets: 20,
contourSortMode: 0,
offsetSinglePoint: { x: 0, y: 0 },
cameraBrightness: 50,
Expand Down Expand Up @@ -166,7 +167,7 @@ export const DefaultReflectivePipelineSettings: ReflectivePipelineSettings = {
cameraGain: 20,
targetModel: TargetModel.InfiniteRechargeHighGoalOuter,
ledMode: true,
outputShowMultipleTargets: false,
outputMaximumTargets: 20,
cameraExposureRaw: 6,
pipelineType: PipelineType.Reflective,

Expand Down Expand Up @@ -197,7 +198,7 @@ export const DefaultColoredShapePipelineSettings: ColoredShapePipelineSettings =
cameraGain: 75,
targetModel: TargetModel.InfiniteRechargeHighGoalOuter,
ledMode: true,
outputShowMultipleTargets: false,
outputMaximumTargets: 20,
cameraExposureRaw: 20,
pipelineType: PipelineType.ColoredShape,

Expand Down Expand Up @@ -237,10 +238,9 @@ export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
cameraGain: 75,
targetModel: TargetModel.AprilTag6p5in_36h11,
ledMode: false,
outputShowMultipleTargets: true,
outputMaximumTargets: 127,
cameraExposureRaw: 20,
pipelineType: PipelineType.AprilTag,

hammingDist: 0,
numIterations: 40,
decimate: 1,
Expand Down Expand Up @@ -278,13 +278,12 @@ export type ConfigurableArucoPipelineSettings = Partial<Omit<ArucoPipelineSettin
export const DefaultArucoPipelineSettings: ArucoPipelineSettings = {
...DefaultPipelineSettings,
cameraGain: 75,
outputShowMultipleTargets: true,
outputMaximumTargets: 127,
targetModel: TargetModel.AprilTag6p5in_36h11,
cameraExposureRaw: -1,
cameraAutoExposure: true,
ledMode: false,
pipelineType: PipelineType.Aruco,

tagFamily: AprilTagFamily.Family36h11,
threshWinSizes: { first: 11, second: 91 },
threshStepSize: 40,
Expand Down Expand Up @@ -316,7 +315,7 @@ export const DefaultObjectDetectionPipelineSettings: ObjectDetectionPipelineSett
cameraGain: 20,
targetModel: TargetModel.InfiniteRechargeHighGoalOuter,
ledMode: true,
outputShowMultipleTargets: false,
outputMaximumTargets: 20,
cameraExposureRaw: 6,
confidence: 0.9,
nms: 0.45,
Expand All @@ -335,7 +334,7 @@ export const DefaultCalibration3dPipelineSettings: Calibration3dPipelineSettings
cameraGain: 20,
targetModel: TargetModel.InfiniteRechargeHighGoalOuter,
ledMode: true,
outputShowMultipleTargets: false,
outputMaximumTargets: 1,
cameraExposureRaw: 6,
drawAllSnapshots: false
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,8 @@ public enum ApriltagTestImages {
kRobots,
kTag1_640_480,
kTag1_16h5_1280,
kTag_corner_1280;
kTag_corner_1280,
k36h11_stress_test;

public final Path path;

Expand All @@ -237,6 +238,7 @@ Path getPath() {
var filename = this.toString().substring(1).toLowerCase();
var extension = ".jpg";
if (filename.equals("tag1_16h5_1280")) extension = ".png";
if (filename.equals("36h11_stress_test")) extension = ".png";
return Path.of("apriltag", filename + extension);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@
public class Draw2dAprilTagsPipe extends Draw2dTargetsPipe {
public static class Draw2dAprilTagsParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
public Draw2dAprilTagsParams(
boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
super(shouldDraw, showMultipleTargets, divisor);
boolean shouldDraw, int outputMaximumTargets, FrameDivisor divisor) {
super(shouldDraw, outputMaximumTargets, divisor);
// We want to show the polygon, not the rotated box
this.showRotatedBox = false;
this.showMaximumBox = false;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,8 @@

public class Draw2dArucoPipe extends Draw2dTargetsPipe {
public static class Draw2dArucoParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
public Draw2dArucoParams(
boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
super(shouldDraw, showMultipleTargets, divisor);
public Draw2dArucoParams(boolean shouldDraw, int outputMaximumTargets, FrameDivisor divisor) {
super(shouldDraw, outputMaximumTargets, divisor);
// We want to show the polygon, not the rotated box
this.showRotatedBox = false;
this.showMaximumBox = false;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -58,14 +58,10 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
var circleColor = ColorHelper.colorToScalar(params.circleColor);
var shapeColour = ColorHelper.colorToScalar(params.shapeOutlineColour);

for (int i = 0; i < (params.showMultipleTargets ? in.getSecond().size() : 1); i++) {
for (int i = 0; i < Math.min(params.outputMaximumTargets, in.getSecond().size()); i++) {
Point[] vertices = new Point[4];
MatOfPoint contour = new MatOfPoint();

if (i != 0 && !params.showMultipleTargets) {
break;
}

TrackedTarget target = in.getSecond().get(i);
RotatedRect r = target.getMinAreaRect();

Expand Down Expand Up @@ -233,8 +229,7 @@ public static class Draw2dTargetsParams {
public Color shapeOutlineColour = Color.MAGENTA;
public Color textColor = Color.GREEN;
public Color circleColor = Color.RED;

public final boolean showMultipleTargets;
public int outputMaximumTargets;
public final boolean shouldDraw;

public final FrameDivisor divisor;
Expand All @@ -247,10 +242,9 @@ public boolean shouldShowCircle(CVShape shape) {
return shape != null && shape.shape.equals(ContourShape.Circle);
}

public Draw2dTargetsParams(
boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
public Draw2dTargetsParams(boolean shouldDraw, int outputMaximumTargets, FrameDivisor divisor) {
this.shouldDraw = shouldDraw;
this.showMultipleTargets = showMultipleTargets;
this.outputMaximumTargets = outputMaximumTargets;
this.divisor = divisor;
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

package org.photonvision.vision.pipeline;

import com.fasterxml.jackson.annotation.JsonAnySetter;
import java.util.Objects;
import org.opencv.core.Point;
import org.photonvision.common.util.numbers.DoubleCouple;
Expand All @@ -41,7 +42,7 @@ public AdvancedPipelineSettings() {
public boolean hueInverted = false;

public boolean outputShouldDraw = true;
public boolean outputShowMultipleTargets = false;
public int outputMaximumTargets = 20;

public DoubleCouple contourArea = new DoubleCouple(0.0, 100.0);
public DoubleCouple contourRatio = new DoubleCouple(0.0, 20.0);
Expand Down Expand Up @@ -90,14 +91,30 @@ public AdvancedPipelineSettings() {
public int cornerDetectionSideCount = 4;
public double cornerDetectionAccuracyPercentage = 10;

/**
* Handles backward compatibility for the deprecated outputShowMultipleTargets property. When
* outputShowMultipleTargets is encountered during deserialization, it sets outputMaximumTargets
* appropriately. If outputShowMultipleTargets is false, outputMaximumTargets is set to 1.
*/
@JsonAnySetter
public void handleUnknownProperty(String name, Object value) {
// Handle the old showMultipleTargets property for backward compatibility
if ("outputShowMultipleTargets".equals(name) && value instanceof Boolean showMultipleTargets) {
if (!showMultipleTargets) {
// If showMultipleTargets is false, limit to 1 target
outputMaximumTargets = 1;
}
}
}

@Override
public boolean equals(Object o) {
if (this == o) return true;
if (!(o instanceof AdvancedPipelineSettings)) return false;
if (!super.equals(o)) return false;
AdvancedPipelineSettings that = (AdvancedPipelineSettings) o;
return outputShouldDraw == that.outputShouldDraw
&& outputShowMultipleTargets == that.outputShowMultipleTargets
&& outputMaximumTargets == that.outputMaximumTargets
&& contourSpecklePercentage == that.contourSpecklePercentage
&& Double.compare(that.offsetDualPointAArea, offsetDualPointAArea) == 0
&& Double.compare(that.offsetDualPointBArea, offsetDualPointBArea) == 0
Expand Down Expand Up @@ -136,7 +153,7 @@ public int hashCode() {
hsvValue,
hueInverted,
outputShouldDraw,
outputShowMultipleTargets,
outputMaximumTargets,
contourArea,
contourRatio,
contourFullness,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,9 @@
import java.util.List;
import java.util.Optional;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.MultiTargetPNPResult;
Expand All @@ -49,6 +52,8 @@
import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;

public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipelineSettings> {
private static final Logger logger = new Logger(AprilTagPipeline.class, LogGroup.VisionModule);

private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
private final AprilTagPoseEstimatorPipe singleTagPoseEstimatorPipe =
new AprilTagPoseEstimatorPipe();
Expand Down Expand Up @@ -232,6 +237,12 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
}
}

if (targetList.size() > Packet.MAX_ARRAY_LEN) {
logger.error(
"We have " + targetList.size() + " targets! Arbitrarily dropping some on the floor");
targetList = targetList.subList(0, Packet.MAX_ARRAY_LEN);
}

var fpsResult = calculateFPSPipe.run(null);
var fps = fpsResult.output;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
public AprilTagPipelineSettings() {
super();
pipelineType = PipelineType.AprilTag;
outputShowMultipleTargets = true;
outputMaximumTargets = 127;
targetModel = TargetModel.kAprilTag6p5in_36h11;
cameraExposureRaw = 20;
cameraAutoExposure = false;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,23 +15,6 @@
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

package org.photonvision.vision.pipeline;

import edu.wpi.first.apriltag.AprilTagPoseEstimate;
Expand All @@ -46,6 +29,9 @@
import org.opencv.imgproc.Imgproc;
import org.opencv.objdetect.Objdetect;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.MultiTargetPNPResult;
Expand All @@ -61,6 +47,8 @@
import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;

public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
private static final Logger logger = new Logger(ArucoPipeline.class, LogGroup.VisionModule);

private ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
private ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe();
private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
Expand Down Expand Up @@ -237,6 +225,12 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
}
}

if (targetList.size() > Packet.MAX_ARRAY_LEN) {
logger.error(
"We have " + targetList.size() + " targets! Arbitrarily dropping some on the floor");
targetList = targetList.subList(0, Packet.MAX_ARRAY_LEN);
}

var fpsResult = calculateFPSPipe.run(null);
var fps = fpsResult.output;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public class ArucoPipelineSettings extends AdvancedPipelineSettings {
public ArucoPipelineSettings() {
super();
pipelineType = PipelineType.Aruco;
outputShowMultipleTargets = true;
outputMaximumTargets = 127;
targetModel = TargetModel.kAprilTag6p5in_36h11;
cameraExposureRaw = 20;
cameraAutoExposure = true;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,6 @@

public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings>
implements Releasable {
static final int MAX_MULTI_TARGET_RESULTS = 50;

protected S settings;
protected FrameStaticProperties frameStaticProperties;
protected QuirkyCamera cameraQuirks;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,11 +101,7 @@ protected void setPipeParamsImpl() {

sortContoursPipe.setParams(
new SortContoursPipe.SortContoursParams(
settings.contourSortMode,
settings.outputShowMultipleTargets
? MAX_MULTI_TARGET_RESULTS // TODO don't hardcode?
: 1,
frameStaticProperties));
settings.contourSortMode, settings.outputMaximumTargets, frameStaticProperties));

collect2dTargetsPipe.setParams(
new Collect2dTargetsPipe.Collect2dTargetsParams(
Expand All @@ -131,7 +127,7 @@ protected void setPipeParamsImpl() {
Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
new Draw2dTargetsPipe.Draw2dTargetsParams(
settings.outputShouldDraw,
settings.outputShowMultipleTargets,
settings.outputMaximumTargets,
settings.streamingFrameDivisor);
draw2DTargetsParams.showShape = true;
draw2DTargetsParams.showMaximumBox = false;
Expand Down
Loading
Loading