From d0e5e169cc381f2862ac60447c5a152b16fb520c Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 27 Dec 2024 20:14:30 -0500 Subject: [PATCH] Fix Aruco leak + remove old tag families (#1661) - Remove 16h5 - Fix leak in ArucoPoseEstimatorPipe --- .../components/dashboard/tabs/AprilTagTab.vue | 2 +- .../components/dashboard/tabs/ArucoTab.vue | 2 +- photon-client/src/types/PipelineTypes.ts | 3 +- .../vision/apriltag/AprilTagFamily.java | 8 +--- .../vision/pipe/impl/ArucoDetectionPipe.java | 1 + .../pipe/impl/ArucoPoseEstimatorPipe.java | 29 +++++++++----- .../vision/pipeline/ArucoPipeline.java | 15 ++----- .../VisionModuleChangeSubscriber.java | 40 +++++++++++-------- 8 files changed, 51 insertions(+), 49 deletions(-) diff --git a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue index a7b017d38..07fa2eee8 100644 --- a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue +++ b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue @@ -27,7 +27,7 @@ const interactiveCols = computed(() => diff --git a/photon-client/src/components/dashboard/tabs/ArucoTab.vue b/photon-client/src/components/dashboard/tabs/ArucoTab.vue index 97573d316..6ebd0897a 100644 --- a/photon-client/src/components/dashboard/tabs/ArucoTab.vue +++ b/photon-client/src/components/dashboard/tabs/ArucoTab.vue @@ -27,7 +27,7 @@ const interactiveCols = computed(() => diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts index b5c7abe1b..65d1b5413 100644 --- a/photon-client/src/types/PipelineTypes.ts +++ b/photon-client/src/types/PipelineTypes.ts @@ -11,8 +11,7 @@ export enum PipelineType { export enum AprilTagFamily { Family36h11 = 0, - Family25h9 = 1, - Family16h5 = 2 + Family16h5 = 1 } export enum RobotOffsetPointMode { diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java index 8fbda0268..7377376e8 100644 --- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java +++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java @@ -19,13 +19,7 @@ package org.photonvision.vision.apriltag; public enum AprilTagFamily { kTag36h11, - kTag25h9, - kTag16h5, - kTagCircle21h7, - kTagCircle49h12, - kTagStandard41h12, - kTagStandard52h13, - kTagCustom48h11; + kTag16h5; public String getNativeName() { // We want to strip the leading kT and replace with "t" diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java index 1b39fd2e4..82dfd31c3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java @@ -101,6 +101,7 @@ public class ArucoDetectionPipe @Override public void setParams(ArucoDetectionPipeParams newParams) { if (this.params == null || !this.params.equals(newParams)) { + System.out.println("Changing tag family to " + newParams.tagFamily); photonDetector .getDetector() .setDictionary(Objdetect.getPredefinedDictionary(newParams.tagFamily)); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java index bc168c6a6..ff9a89319 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java @@ -52,6 +52,9 @@ public class ArucoPoseEstimatorPipe // reprojection error of solvepnp estimations private final Mat reprojectionErrors = Mat.zeros(2, 1, CvType.CV_32F); + // Tag corner locations in object space - order matters for ippe_square + MatOfPoint3f objectPoints = new MatOfPoint3f(); + private final int kNaNRetries = 1; private Translation3d tvecToTranslation3d(Mat mat) { @@ -81,7 +84,7 @@ public class ArucoPoseEstimatorPipe for (int i = 0; i < kNaNRetries + 1; i++) { // SolvePnP with SOLVEPNP_IPPE_SQUARE solver Calib3d.solvePnPGeneric( - params.objectPoints, + objectPoints, imagePoints, params.calibration.getCameraIntrinsicsMat(), params.calibration.getDistCoeffsMat(), @@ -116,6 +119,19 @@ public class ArucoPoseEstimatorPipe @Override public void setParams(ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams newParams) { + // exact equality check OK here, the number shouldn't change + if (this.params == null || this.params.tagSize != newParams.tagSize) { + var tagSize = newParams.tagSize; + + // This order is relevant for SOLVEPNP_IPPE_SQUARE + // The expected 2d correspondences with a tag facing the camera would be (BR, BL, TL, TR) + objectPoints.fromArray( + new Point3(-tagSize / 2, tagSize / 2, 0), + new Point3(tagSize / 2, tagSize / 2, 0), + new Point3(tagSize / 2, -tagSize / 2, 0), + new Point3(-tagSize / 2, -tagSize / 2, 0)); + } + super.setParams(newParams); } @@ -134,21 +150,12 @@ public class ArucoPoseEstimatorPipe public static class ArucoPoseEstimatorPipeParams { final CameraCalibrationCoefficients calibration; final double tagSize; + // object vertices defined by tag size - final MatOfPoint3f objectPoints; public ArucoPoseEstimatorPipeParams(CameraCalibrationCoefficients cal, double tagSize) { this.calibration = cal; this.tagSize = tagSize; - - // This order is relevant for SOLVEPNP_IPPE_SQUARE - // The expected 2d correspondences with a tag facing the camera would be (BR, BL, TL, TR) - objectPoints = - new MatOfPoint3f( - new Point3(-tagSize / 2, tagSize / 2, 0), - new Point3(tagSize / 2, tagSize / 2, 0), - new Point3(tagSize / 2, -tagSize / 2, 0), - new Point3(-tagSize / 2, -tagSize / 2, 0)); } } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index 9a51f6960..8395054a1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -94,18 +94,11 @@ public class ArucoPipeline extends CVPipeline Objdetect.DICT_APRILTAG_25h9; - // TODO: explicitly drop support for these - case kTag16h5, - kTagCircle21h7, - kTagCircle49h12, - kTagCustom48h11, - kTagStandard41h12, - kTagStandard52h13 -> { + case kTag16h5 -> { // 2024 tag, 6.5in - tagWidth = Units.inchesToMeters(6.5); - tagModel = TargetModel.kAprilTag36h11; - yield Objdetect.DICT_APRILTAG_36h11; + tagWidth = Units.inchesToMeters(6); + tagModel = TargetModel.kAprilTag16h5; + yield Objdetect.DICT_APRILTAG_16h5; } }; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index 0b6f1c58c..1094c3a3a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -89,8 +89,12 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber { var newPropValue = change.getNewPropValue(); var currentSettings = change.getCurrentSettings(); var originContext = change.getOriginContext(); + boolean handled = true; switch (propName) { - case "pipelineName" -> newPipelineNickname((String) newPropValue); + case "pipelineName" -> { + newPipelineNickname((String) newPropValue); + continue; + } case "newPipelineInfo" -> newPipelineInfo((Pair) newPropValue); case "deleteCurrPipeline" -> deleteCurrPipeline(); case "changePipeline" -> changePipeline((Integer) newPropValue); @@ -116,6 +120,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber { parentModule.saveAndBroadcastAll(); } case "isDriverMode" -> parentModule.setDriverMode((Boolean) newPropValue); + default -> handled = false; } // special case for camera settables @@ -133,21 +138,24 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber { } } - try { - setProperty(currentSettings, propName, newPropValue); - logger.trace("Set prop " + propName + " to value " + newPropValue); - } catch (NoSuchFieldException | IllegalAccessException e) { - logger.error( - "Could not set prop " - + propName - + " with value " - + newPropValue - + " on " - + currentSettings - + " | " - + e.getClass().getSimpleName()); - } catch (Exception e) { - logger.error("Unknown exception when setting PSC prop!", e); + if (!handled) { + try { + setProperty(currentSettings, propName, newPropValue); + logger.trace("Set prop " + propName + " to value " + newPropValue); + } catch (NoSuchFieldException | IllegalAccessException e) { + logger.error( + "Could not set prop " + + propName + + " with value " + + newPropValue + + " on " + + currentSettings + + " | " + + e.getClass().getSimpleName(), + e); + } catch (Exception e) { + logger.error("Unknown exception when setting PSC prop!", e); + } } parentModule.saveAndBroadcastSelective(originContext, propName, newPropValue);