From 23f3c0e6e155f3b00a498543b99fb2c3dd7b4f19 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 13 Oct 2020 06:58:50 -0700 Subject: [PATCH] Use chessboard squares (vs interior corners); fix resolution selector bug (#139) * Change chessboard size to be squares not interior corners This reduces ambiguity * Force users to select resolution This forces the correct video mode index to be selected. Otherwise the 0th camera videomode index will be used, as it's zero-inited. This is undesirable. * Make target model an enum This will allow the UI to remember the currently selected target. --- photon-client/src/assets/FRCtargets.json | 52 ------- photon-client/src/store/index.js | 4 +- photon-client/src/views/CamerasView.vue | 7 +- .../src/views/PipelineViews/PnPTab.vue | 64 ++------ .../src/main/java/org/photonvision/Main.java | 7 +- .../pipe/impl/FindBoardCornersPipe.java | 46 ++++-- .../Calibration3dPipelineSettings.java | 4 +- .../pipeline/ReflectivePipelineSettings.java | 2 +- .../vision/target/TargetModel.java | 145 ++++++++++-------- .../common/configuration/ConfigTest.java | 2 +- .../vision/pipeline/CirclePNPTest.java | 2 +- .../vision/pipeline/SolvePNPTest.java | 4 +- .../resources/hardware/HardwareConfig.json | 1 - 13 files changed, 146 insertions(+), 194 deletions(-) delete mode 100644 photon-client/src/assets/FRCtargets.json diff --git a/photon-client/src/assets/FRCtargets.json b/photon-client/src/assets/FRCtargets.json deleted file mode 100644 index 2aa605e0c..000000000 --- a/photon-client/src/assets/FRCtargets.json +++ /dev/null @@ -1,52 +0,0 @@ -{ - "2020 Hex Goal": { - "realWorldCoordinatesArray": [ - { - "x": -0.49847498536109924, - "y": 0.0, - "z": 0.0 - }, - { - "x": -0.24942462146282196, - "y": -0.4318000078201294, - "z": 0.0 - }, - { - "x": 0.24942462146282196, - "y": -0.4318000078201294, - "z": 0.0 - }, - { - "x": 0.49847498536109924, - "y": 0.0, - "z": 0.0 - } - ], - "boxHeight": 0.30479999999999996 - }, - "2019 Dual Target": { - "realWorldCoordinatesArray": [ - { - "x": -0.15077440440654755, - "y": 0.06761480122804642, - "z": 0.0 - }, - { - "x": -0.18575020134449005, - "y": -0.06761480122804642, - "z": 0.0 - }, - { - "x": 0.18575020134449005, - "y": -0.06761480122804642, - "z": 0.0 - }, - { - "x": 0.15077440440654755, - "y": 0.06761480122804642, - "z": 0.0 - } - ], - "boxHeight": 0.1 - } -} \ No newline at end of file diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js index ae8559875..6b17b8e30 100644 --- a/photon-client/src/store/index.js +++ b/photon-client/src/store/index.js @@ -122,8 +122,8 @@ export default new Vuex.Store({ minCount: 25, hasEnough: false, squareSizeIn: 1.0, - patternWidth: 7, - patternHeight: 7, + patternWidth: 8, + patternHeight: 8, boardType: 0, // Chessboard, dotboard }, metrics: { diff --git a/photon-client/src/views/CamerasView.vue b/photon-client/src/views/CamerasView.vue index ebbe5d80b..5b385ee3f 100644 --- a/photon-client/src/views/CamerasView.vue +++ b/photon-client/src/views/CamerasView.vue @@ -93,14 +93,14 @@ v-model="boardWidth" name="Board width" label-cols="5" - tooltip="Width of the board in dots or corners; with the standard chessboard, this is usually 7" + tooltip="Width of the board in dots or chessboard squares; with the standard chessboard, this is usually 8" :disabled="isCalibrating" /> @@ -291,7 +291,7 @@ export default { text: "" }, snack: false, - filteredVideomodeIndex: 0 + filteredVideomodeIndex: undefined, } }, computed: { @@ -409,7 +409,6 @@ export default { return this.$store.getters.currentPipelineIndex === -2; } }, - selectedFilteredResIndex: { get() { return this.filteredVideomodeIndex diff --git a/photon-client/src/views/PipelineViews/PnPTab.vue b/photon-client/src/views/PipelineViews/PnPTab.vue index d93ef2e88..7c76c62b3 100644 --- a/photon-client/src/views/PipelineViews/PnPTab.vue +++ b/photon-client/src/views/PipelineViews/PnPTab.vue @@ -16,10 +16,10 @@ color="accent" item-color="secondary" label="Select a target model" - :items="FRCtargets" + :items="targetList" item-text="name" item-value="data" - @change="onModelSelect" + @input="handlePipelineUpdate('targetModel', targetList.indexOf(selectedModel))" /> 0) { let data = []; @@ -158,29 +143,6 @@ this.selectedModel = null; } }, - uploadPremade() { - this.uploadModel(this.selectedModel, true); - }, - uploadModel(model, premade = false) { - this.axios.post("http://" + this.$address + "/api/vision/pnpModel", { - ['targetModel']: model, - ['index']: this.$store.getters.currentCameraIndex - }).then(() => { - this.snackbar = { - color: "success", - text: premade ? "Target model changed successfully" : "Custom target model uploaded and selected successfully" - }; - this.snack = true; - }).catch(() => { - this.snackbar = { - color: "error", - text: "An error occurred selecting a target model" - }; - this.snack = true; - - this.selectedModel = null; - }); - } } } diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 5ecde2a02..a235dfc7f 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -33,6 +33,7 @@ import org.photonvision.common.networking.NetworkManager; import org.photonvision.common.util.TestUtils; import org.photonvision.server.Server; import org.photonvision.vision.camera.FileVisionSource; +import org.photonvision.vision.opencv.ContourGroupingMode; import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; import org.photonvision.vision.processes.VisionModule; @@ -93,7 +94,9 @@ public class Main { var pipeline2019 = new ReflectivePipelineSettings(); pipeline2019.pipelineNickname = "CargoShip"; - pipeline2019.targetModel = TargetModel.get2019Target(); + pipeline2019.targetModel = TargetModel.k2019DualTarget; + pipeline2019.outputShowMultipleTargets = true; + pipeline2019.contourGroupingMode = ContourGroupingMode.Dual; var psList2019 = new ArrayList(); psList2019.add(pipeline2019); @@ -107,7 +110,7 @@ public class Main { var pipeline2020 = new ReflectivePipelineSettings(); pipeline2020.pipelineNickname = "OuterPort"; - pipeline2020.targetModel = TargetModel.get2020Target(); + pipeline2020.targetModel = TargetModel.k2020HighGoalOuter; camConf2020.calibrations.add(TestUtils.get2020LifeCamCoeffs(true)); var psList2020 = new ArrayList(); diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java index 628f786e5..f38cbe73b 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java @@ -17,15 +17,21 @@ package org.photonvision.vision.pipe.impl; +import java.util.Objects; import org.apache.commons.lang3.tuple.Triple; import org.opencv.calib3d.Calib3d; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.pipeline.UICalibrationData; public class FindBoardCornersPipe extends CVPipe, FindBoardCornersPipe.FindCornersPipeParams> { + private static final Logger logger = + new Logger(FindBoardCornersPipe.class, LogGroup.VisionModule); + MatOfPoint3f objectPoints = new MatOfPoint3f(); Size imageSize; @@ -38,25 +44,22 @@ public class FindBoardCornersPipe private final Size zeroZone = new Size(-1, -1); private final TermCriteria criteria = new TermCriteria(3, 30, 0.001); - private boolean objectPointsCreated = false; - - @Override - public void setParams(FindCornersPipeParams params) { - super.setParams(params); - - if (new Size(params.boardWidth, params.boardHeight).equals(patternSize)) return; - - objectPointsCreated = false; - } + private FindCornersPipeParams lastParams = null; public void createObjectPoints() { - if (objectPointsCreated) return; // TODO reinstantiate on settings change + if (this.lastParams != null && this.lastParams.equals(this.params)) return; + this.lastParams = this.params; /*If using a chessboard, then the pattern size if the inner corners of the board. For example, the pattern size of a 9x9 chessboard would be 8x8 If using a dot board, then the pattern size width is the sum of the bottom 2 rows and the height is the left or right most column For example, a 5x4 dot board would have a pattern size of 11x4 + We subtract 1 for chessboard because the UI prompts users for the number of squares, not the + number of corners. * */ - this.patternSize = new Size(params.boardWidth, params.boardHeight); + this.patternSize = + params.type == UICalibrationData.BoardType.CHESSBOARD + ? new Size(params.boardWidth - 1, params.boardHeight - 1) + : new Size(params.boardWidth, params.boardHeight); // Chessboard and dot board have different 3D points to project as a dot board has alternating // dots per column @@ -81,9 +84,8 @@ public class FindBoardCornersPipe } } } else { - // TOOD log + logger.error("Can't create pattern for unknown board type " + params.type); } - objectPointsCreated = true; } /** @@ -162,5 +164,21 @@ public class FindBoardCornersPipe this.type = type; this.gridSize = gridSize; // mm } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + FindCornersPipeParams that = (FindCornersPipeParams) o; + return boardHeight == that.boardHeight + && boardWidth == that.boardWidth + && Double.compare(that.gridSize, gridSize) == 0 + && type == that.type; + } + + @Override + public int hashCode() { + return Objects.hash(boardHeight, boardWidth, type, gridSize); + } } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java index 546dd6f3b..698a354f2 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java @@ -21,8 +21,8 @@ import edu.wpi.first.wpilibj.util.Units; import org.opencv.core.Size; public class Calibration3dPipelineSettings extends AdvancedPipelineSettings { - public int boardHeight = 7; - public int boardWidth = 7; + public int boardHeight = 8; + public int boardWidth = 8; public UICalibrationData.BoardType boardType = UICalibrationData.BoardType.CHESSBOARD; public double gridSize = Units.inchesToMeters(1.0); diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java index f93b869ba..b314e857d 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java @@ -34,7 +34,7 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings { // 3d settings public boolean solvePNPEnabled = false; - public TargetModel targetModel = TargetModel.get2020Target(); + public TargetModel targetModel = TargetModel.k2020HighGoalOuter; // Corner detection settings public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy = diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java index 2d87ec9aa..634e15fe3 100644 --- a/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -23,21 +23,71 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.wpilibj.util.Units; import java.util.ArrayList; import java.util.List; -import java.util.Objects; import org.opencv.core.MatOfPoint3f; import org.opencv.core.Point3; import org.photonvision.vision.opencv.Releasable; -public class TargetModel implements Releasable { +public enum TargetModel implements Releasable { + k2020HighGoalOuter( + List.of( + new Point3(Units.inchesToMeters(-19.625), 0, 0), + new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(-17), 0), + new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(-17), 0), + new Point3(Units.inchesToMeters(19.625), 0, 0)), + Units.inchesToMeters(12)), + k2020HighGoalInner( + List.of( + new Point3(Units.inchesToMeters(-19.625), 0, Units.inchesToMeters(2d * 12d + 5.25)), + new Point3( + Units.inchesToMeters(-9.819867), + Units.inchesToMeters(-17), + Units.inchesToMeters(2d * 12d + 5.25)), + new Point3( + Units.inchesToMeters(9.819867), + Units.inchesToMeters(-17), + Units.inchesToMeters(2d * 12d + 5.25)), + new Point3(Units.inchesToMeters(19.625), 0, Units.inchesToMeters(2d * 12d + 5.25))), + Units.inchesToMeters(12)), + k2019DualTarget( + List.of( + new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(2.662), 0), + new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(-2.662), 0), + new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(-2.662), 0), + new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(2.662), 0)), + 0.1), + kCircularPowerCell7in( + List.of( + new Point3( + -Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2), + new Point3( + -Units.inchesToMeters(7) / 2, + Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2), + new Point3( + Units.inchesToMeters(7) / 2, + Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2), + new Point3( + Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2)), + 0); - @JsonIgnore private final MatOfPoint3f realWorldTargetCoordinates; - @JsonIgnore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); - @JsonIgnore private final MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); + @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates; + @JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); + @JsonIgnore private MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); - public final List realWorldCoordinatesArray; - public final double boxHeight; + @JsonProperty("realWorldCoordinatesArray") + private List realWorldCoordinatesArray; - public TargetModel(MatOfPoint3f realWorldTargetCoordinates, double boxHeight) { + @JsonProperty("boxHeight") + private double boxHeight; + + TargetModel() {} + + TargetModel(MatOfPoint3f realWorldTargetCoordinates, double boxHeight) { this.realWorldTargetCoordinates = realWorldTargetCoordinates; this.realWorldCoordinatesArray = realWorldTargetCoordinates.toList(); this.boxHeight = boxHeight; @@ -53,12 +103,28 @@ public class TargetModel implements Releasable { } @JsonCreator - public TargetModel( + TargetModel( @JsonProperty(value = "realWorldCoordinatesArray") List points, @JsonProperty(value = "boxHeight") double boxHeight) { this(listToMat(points), boxHeight); } + public List getRealWorldCoordinatesArray() { + return this.realWorldCoordinatesArray; + } + + public double getBoxHeight() { + return boxHeight; + } + + public void setRealWorldCoordinatesArray(List realWorldCoordinatesArray) { + this.realWorldCoordinatesArray = realWorldCoordinatesArray; + } + + public void setBoxHeight(double boxHeight) { + this.boxHeight = boxHeight; + } + private static MatOfPoint3f listToMat(List points) { var mat = new MatOfPoint3f(); mat.fromList(points); @@ -77,58 +143,15 @@ public class TargetModel implements Releasable { return visualizationBoxTop; } - public static TargetModel get2020Target() { - return get2020Target(0); - } - - public static TargetModel get2020TargetInnerPort() { - // Per the game manual, the inner port is 2ft 5.25in behind the outer port - return get2020Target(Units.inchesToMeters(2d * 12d + 5.25)); - } - - public static TargetModel get2020Target(double offsetMeters) { - var corners = - List.of( - new Point3(Units.inchesToMeters(-19.625), 0, offsetMeters), - new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(-17), offsetMeters), - new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(-17), offsetMeters), - new Point3(Units.inchesToMeters(19.625), 0, offsetMeters)); - return new TargetModel(corners, Units.inchesToMeters(12)); - } - - public static TargetModel get2019Target() { - var corners = - List.of( - new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(2.662), 0), - new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(-2.662), 0), - new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(-2.662), 0), - new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(2.662), 0)); - return new TargetModel(corners, 0.1); - } - - public static TargetModel getCircleTarget(double radius) { - var corners = - List.of( - new Point3(-radius / 2, -radius / 2, -radius / 2), - new Point3(-radius / 2, radius / 2, -radius / 2), - new Point3(radius / 2, radius / 2, -radius / 2), - new Point3(radius / 2, -radius / 2, -radius / 2)); - return new TargetModel(corners, 0); - } - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (!(o instanceof TargetModel)) return false; - TargetModel that = (TargetModel) o; - return Double.compare(that.boxHeight, boxHeight) == 0 - && Objects.equals(realWorldCoordinatesArray, that.realWorldCoordinatesArray); - } - - @Override - public int hashCode() { - return Objects.hash(realWorldCoordinatesArray, boxHeight); - } + // public static TargetModel getCircleTarget(double Units.inchesToMeters(7)) { + // var corners = + // List.of( + // new Point3(-Units.inchesToMeters(7) / 2, -radius / 2, -radius / 2), + // new Point3(-Units.inchesToMeters(7) / 2, radius / 2, -radius / 2), + // new Point3(Units.inchesToMeters(7) / 2, radius / 2, -radius / 2), + // new Point3(Units.inchesToMeters(7) / 2, -radius / 2, -radius / 2)); + // return new TargetModel(corners, 0); + // } @Override public void release() { diff --git a/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java index 646693f26..7622780c3 100644 --- a/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java +++ b/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java @@ -54,7 +54,7 @@ public class ConfigTest { COLORED_SHAPE_PIPELINE_SETTINGS = new ColoredShapePipelineSettings(); REFLECTIVE_PIPELINE_SETTINGS.pipelineNickname = "2019Tape"; - REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.get2019Target(); + REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.k2019DualTarget; COLORED_SHAPE_PIPELINE_SETTINGS.pipelineNickname = "2019Cargo"; COLORED_SHAPE_PIPELINE_SETTINGS.pipelineIndex = 1; diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java index e62f4e4d4..1fbe2bade 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java @@ -97,7 +97,7 @@ public class CirclePNPTest { pipeline.getSettings().cornerDetectionAccuracyPercentage = 4; pipeline.getSettings().cornerDetectionUseConvexHulls = true; pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_480P_CAL_FILE); - pipeline.getSettings().targetModel = TargetModel.getCircleTarget(7); + pipeline.getSettings().targetModel = TargetModel.kCircularPowerCell7in; pipeline.getSettings().cameraPitch = Rotation2d.fromDegrees(0.0); pipeline.getSettings().outputShouldDraw = true; pipeline.getSettings().outputShowMultipleTargets = false; diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index 36e1d52ca..1d339e450 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java @@ -98,7 +98,7 @@ public class SolvePNPTest { pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Dual; pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up; pipeline.getSettings().cornerDetectionUseConvexHulls = true; - pipeline.getSettings().targetModel = TargetModel.get2019Target(); + pipeline.getSettings().targetModel = TargetModel.k2019DualTarget; var frameProvider = new FileFrameProvider( @@ -133,7 +133,7 @@ public class SolvePNPTest { pipeline.getSettings().solvePNPEnabled = true; pipeline.getSettings().cornerDetectionAccuracyPercentage = 4; pipeline.getSettings().cornerDetectionUseConvexHulls = true; - pipeline.getSettings().targetModel = TargetModel.get2020Target(); + pipeline.getSettings().targetModel = TargetModel.k2020HighGoalOuter; var frameProvider = new FileFrameProvider( diff --git a/photon-server/src/test/resources/hardware/HardwareConfig.json b/photon-server/src/test/resources/hardware/HardwareConfig.json index a97ba2ac5..acfc790fd 100644 --- a/photon-server/src/test/resources/hardware/HardwareConfig.json +++ b/photon-server/src/test/resources/hardware/HardwareConfig.json @@ -15,7 +15,6 @@ "cpuMemoryCommand" : "echo 10", "cpuUtilCommand" : "echo 10", "gpuMemoryCommand" : "echo 10", - "gpuTempCommand" : "echo 10", "ramUtilCommand" : "echo 10", "restartHardwareCommand" : "echo 10", "vendorFOV" : 40.0