diff --git a/build.gradle b/build.gradle index 34943ce37..a95bfc2de 100644 --- a/build.gradle +++ b/build.gradle @@ -15,6 +15,7 @@ import org.gradle.api.internal.artifacts.dependencies.DefaultExternalModuleDepen allprojects { repositories { mavenCentral() + mavenLocal() maven { url = "https://maven.photonvision.org/repository/internal/" } } wpilibRepositories.addAllReleaseRepositories(it) @@ -25,7 +26,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2023.1.1-beta-7" + wpilibVersion = "2023.1.1-beta-7-15-g1e7fcd5" opencvVersion = "4.6.0-4" joglVersion = "2.4.0-rc-20200307" pubVersion = versionString diff --git a/photon-core/build.gradle b/photon-core/build.gradle index e6499eefc..782e16a21 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -23,6 +23,8 @@ dependencies { // Zip implementation 'org.zeroturnaround:zt-zip:1.14' + + implementation wpilibTools.deps.wpilibJava("apriltag") } task writeCurrentVersionJava { diff --git a/photon-core/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/photon-core/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java deleted file mode 100644 index d3a2cf4ff..000000000 --- a/photon-core/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java +++ /dev/null @@ -1,102 +0,0 @@ -/* - * 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 . - */ - -package edu.wpi.first.apriltag.jni; - -import edu.wpi.first.util.RuntimeLoader; -import java.io.IOException; -import java.util.concurrent.atomic.AtomicBoolean; -import org.opencv.core.Mat; - -public class AprilTagJNI { - static boolean libraryLoaded = false; - - static RuntimeLoader loader = null; - - public static class Helper { - private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); - - public static boolean getExtractOnStaticLoad() { - return extractOnStaticLoad.get(); - } - - public static void setExtractOnStaticLoad(boolean load) { - extractOnStaticLoad.set(load); - } - } - - static { - if (Helper.getExtractOnStaticLoad()) { - try { - loader = - new RuntimeLoader<>( - "apriltagjni", RuntimeLoader.getDefaultExtractionRoot(), AprilTagJNI.class); - loader.loadLibrary(); - } catch (IOException ex) { - ex.printStackTrace(); - System.exit(1); - } - libraryLoaded = true; - } - } - - // Returns a pointer to a apriltag_detector_t - public static native long aprilTagCreate( - String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges); - - // Destroy and free a previously created detector. - public static native void aprilTagDestroy(long detector); - - private static native Object[] aprilTagDetectInternal( - long detector, - long imgAddr, - int rows, - int cols, - boolean doPoseEstimation, - double tagWidth, - double fx, - double fy, - double cx, - double cy, - int nIters); - - // Detect targets given a GRAY frame. Returns a pointer toa zarray - public static DetectionResult[] aprilTagDetect( - long detector, - Mat img, - boolean doPoseEstimation, - double tagWidth, - double fx, - double fy, - double cx, - double cy, - int nIters) { - return (DetectionResult[]) - aprilTagDetectInternal( - detector, - img.dataAddr(), - img.rows(), - img.cols(), - doPoseEstimation, - tagWidth, - fx, - fy, - cx, - cy, - nIters); - } -} diff --git a/photon-core/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java b/photon-core/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java deleted file mode 100644 index 6df6b2038..000000000 --- a/photon-core/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java +++ /dev/null @@ -1,246 +0,0 @@ -/* - * 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 . - */ - -package edu.wpi.first.apriltag.jni; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N3; -import java.util.Arrays; -import org.ejml.data.DMatrixRMaj; -import org.ejml.dense.row.factory.DecompositionFactory_DDRM; -import org.ejml.simple.SimpleMatrix; - -public class DetectionResult { - public int getId() { - return m_id; - } - - public int getHamming() { - return m_hamming; - } - - public float getDecisionMargin() { - return m_decisionMargin; - } - - public void setDecisionMargin(float decisionMargin) { - this.m_decisionMargin = decisionMargin; - } - - @SuppressWarnings("PMD.MethodReturnsInternalArray") - public double[] getHomography() { - return m_homography; - } - - @SuppressWarnings("PMD.ArrayIsStoredDirectly") - public void setHomography(double[] homography) { - this.m_homography = homography; - } - - public double getCenterX() { - return m_centerX; - } - - public void setCenterX(double centerX) { - this.m_centerX = centerX; - } - - public double getCenterY() { - return m_centerY; - } - - public void setCenterY(double centerY) { - this.m_centerY = centerY; - } - - @SuppressWarnings("PMD.MethodReturnsInternalArray") - public double[] getCorners() { - return m_corners; - } - - @SuppressWarnings("PMD.ArrayIsStoredDirectly") - public void setCorners(double[] corners) { - this.m_corners = corners; - } - - public double getError1() { - return m_error1; - } - - public double getError2() { - return m_error2; - } - - public Transform3d getPoseResult1() { - return m_poseResult1; - } - - public Transform3d getPoseResult2() { - return m_poseResult2; - } - - private final int m_id; - private final int m_hamming; - private float m_decisionMargin; - private double[] m_homography; - private double m_centerX; - private double m_centerY; - private double[] m_corners; - - private final Transform3d m_poseResult1; - private final double m_error1; - private final Transform3d m_poseResult2; - private final double m_error2; - - /** - * Constructs a new detection result. Used from JNI. - * - * @param id id - * @param hamming hamming - * @param decisionMargin dm - * @param homography homography - * @param centerX centerX - * @param centerY centerY - * @param corners corners - * @param pose1TransArr pose1TransArr - * @param pose1RotArr pose1RotArr - * @param err1 err1 - * @param pose2TransArr pose2TransArr - * @param pose2RotArr pose2RotArr - * @param err2 err2 - */ - @SuppressWarnings("PMD.ArrayIsStoredDirectly") - public DetectionResult( - int id, - int hamming, - float decisionMargin, - double[] homography, - double centerX, - double centerY, - double[] corners, - double[] pose1TransArr, - double[] pose1RotArr, - double err1, - double[] pose2TransArr, - double[] pose2RotArr, - double err2) { - this.m_id = id; - this.m_hamming = hamming; - this.m_decisionMargin = decisionMargin; - this.m_homography = homography; - this.m_centerX = centerX; - this.m_centerY = centerY; - this.m_corners = corners; - - this.m_error1 = err1; - var rot1 = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr); - if (rot1.normF() > 0) { - this.m_poseResult1 = - new Transform3d( - new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]), - new Rotation3d(orthogonalizeRotationMatrix(rot1))); - } else { - this.m_poseResult1 = new Transform3d(); - } - - this.m_error2 = err2; - var rot2 = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr); - if (rot2.normF() > 0) { - this.m_poseResult2 = - new Transform3d( - new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]), - new Rotation3d(orthogonalizeRotationMatrix(rot2))); - } else { - this.m_poseResult2 = new Transform3d(); - } - } - - /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. - * - * @return The ratio of pose reprojection errors. - */ - public double getPoseAmbiguity() { - var min = Math.min(m_error1, m_error2); - var max = Math.max(m_error1, m_error2); - - if (max > 0) { - return min / max; - } else { - return -1; - } - } - - @Override - public String toString() { - return "DetectionResult [centerX=" - + m_centerX - + ", centerY=" - + m_centerY - + ", corners=" - + Arrays.toString(m_corners) - + ", decisionMargin=" - + m_decisionMargin - + ", error1=" - + m_error1 - + ", error2=" - + m_error2 - + ", hamming=" - + m_hamming - + ", homography=" - + Arrays.toString(m_homography) - + ", id=" - + m_id - + ", poseResult1=" - + m_poseResult1 - + ", poseResult2=" - + m_poseResult2 - + "]"; - } - - private static Matrix orthogonalizeRotationMatrix(Matrix input) { - var a = DecompositionFactory_DDRM.qr(3, 3); - if (!a.decompose(input.getStorage().getDDRM())) { - // best we can do is return the input - return input; - } - - // Grab results (thanks for this _great_ api, EJML) - var Q = new DMatrixRMaj(3, 3); - var R = new DMatrixRMaj(3, 3); - a.getQ(Q, false); - a.getR(R, false); - - // Fix signs in R if they're < 0 so it's close to an identity matrix - // (our QR decomposition implementation sometimes flips the signs of columns) - for (int colR = 0; colR < 3; ++colR) { - if (R.get(colR, colR) < 0) { - for (int rowQ = 0; rowQ < 3; ++rowQ) { - Q.set(rowQ, colR, -Q.get(rowQ, colR)); - } - } - } - - return new Matrix<>(new SimpleMatrix(Q)); - } -} diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/.styleguide-license b/photon-core/src/main/java/org/photonvision/vision/apriltag/.styleguide-license deleted file mode 100644 index 7ac216bf5..000000000 --- a/photon-core/src/main/java/org/photonvision/vision/apriltag/.styleguide-license +++ /dev/null @@ -1,25 +0,0 @@ -/* -Copyright (c) {year} Photon Vision. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java deleted file mode 100644 index 0ff18937d..000000000 --- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java +++ /dev/null @@ -1,103 +0,0 @@ -/* -Copyright (c) 2022 Photon Vision. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -package org.photonvision.vision.apriltag; - -import org.opencv.core.Mat; -import org.photonvision.common.logging.LogGroup; -import org.photonvision.common.logging.Logger; -import org.photonvision.vision.calibration.CameraCalibrationCoefficients; -import edu.wpi.first.apriltag.jni.AprilTagJNI; -import edu.wpi.first.apriltag.jni.DetectionResult; - -public class AprilTagDetector { - private static final Logger logger = new Logger(AprilTagDetector.class, LogGroup.VisionModule); - private long m_detectorPtr = 0; - private AprilTagDetectorParams m_detectorParams = AprilTagDetectorParams.DEFAULT_36H11; - - public AprilTagDetector() { - updateDetector(); - } - - private void updateDetector() { - if (m_detectorPtr != 0) { - // TODO: in JNI - AprilTagJNI.aprilTagDestroy(m_detectorPtr); - m_detectorPtr = 0; - } - - logger.debug("Creating detector with params " + m_detectorParams); - m_detectorPtr = - AprilTagJNI.aprilTagCreate( - m_detectorParams.tagFamily.getNativeName(), - m_detectorParams.decimate, - m_detectorParams.blur, - m_detectorParams.threads, - m_detectorParams.debug, - m_detectorParams.refineEdges); - } - - public void updateParams(AprilTagDetectorParams newParams) { - if (!m_detectorParams.equals(newParams)) { - m_detectorParams = newParams; - updateDetector(); - } - } - - public DetectionResult[] detect( - Mat grayscaleImg, - CameraCalibrationCoefficients coeffs, - boolean useNativePoseEst, - int numIterations, - double tagWidthMeters) { - if (m_detectorPtr == 0) { - // Detector not set up (JNI issue? or similar?) - // No detection is possible. - return new DetectionResult[] {}; - } - - var cx = 0.0; - var cy = 0.0; - var fx = 0.0; - var fy = 0.0; - var doPoseEst = false; - - if (coeffs != null && useNativePoseEst) { - final Mat cameraMatrix = coeffs.getCameraIntrinsicsMat(); - if (cameraMatrix != null) { - // Camera calibration has been done, we should be able to do pose estimation - cx = cameraMatrix.get(0, 2)[0]; - cy = cameraMatrix.get(1, 2)[0]; - fx = cameraMatrix.get(0, 0)[0]; - fy = cameraMatrix.get(1, 1)[0]; - doPoseEst = true; - } - } - - return AprilTagJNI.aprilTagDetect( - m_detectorPtr, grayscaleImg, doPoseEst, tagWidthMeters, fx, fy, cx, cy, numIterations); - } -} diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java b/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java deleted file mode 100644 index 8942b50f6..000000000 --- a/photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java +++ /dev/null @@ -1,87 +0,0 @@ -/* -Copyright (c) 2022 Photon Vision. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -package org.photonvision.vision.apriltag; - -import java.util.Objects; - -public class AprilTagDetectorParams { - public static AprilTagDetectorParams DEFAULT_36H11 = - new AprilTagDetectorParams(AprilTagFamily.kTag36h11, 1.0, 0.0, 4, false, false); - - public final AprilTagFamily tagFamily; - public final double decimate; - public final double blur; - public final int threads; - public final boolean debug; - public final boolean refineEdges; - - public AprilTagDetectorParams( - AprilTagFamily tagFamily, - double decimate, - double blur, - int threads, - boolean debug, - boolean refineEdges) { - this.tagFamily = tagFamily; - this.decimate = decimate; - this.blur = blur; - this.threads = threads; - this.debug = debug; - this.refineEdges = refineEdges; - } - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - AprilTagDetectorParams that = (AprilTagDetectorParams) o; - return Objects.equals(tagFamily, that.tagFamily) - && Double.compare(decimate, that.decimate) == 0 - && Double.compare(blur, that.blur) == 0 - && threads == that.threads - && debug == that.debug - && refineEdges == that.refineEdges; - } - - @Override - public String toString() { - return "AprilTagDetectorParams{" - + "tagFamily=" - + tagFamily.getNativeName() - + ", decimate=" - + decimate - + ", blur=" - + blur - + ", threads=" - + threads - + ", debug=" - + debug - + ", refineEdges=" - + refineEdges - + '}'; - } -} 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 a3361410c..afbfc463a 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 @@ -1,28 +1,19 @@ /* -Copyright (c) 2022 Photon Vision. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ + * 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 . + */ package org.photonvision.vision.apriltag; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java index d41f2cc11..5f2662754 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java @@ -17,33 +17,46 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.apriltag.jni.DetectionResult; +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; import java.util.List; import org.opencv.core.Mat; -import org.photonvision.vision.apriltag.AprilTagDetector; import org.photonvision.vision.pipe.CVPipe; public class AprilTagDetectionPipe - extends CVPipe, AprilTagDetectionPipeParams> { + extends CVPipe, AprilTagDetectionPipeParams> { private final AprilTagDetector m_detector = new AprilTagDetector(); boolean useNativePoseEst; - @Override - protected List process(Mat in) { - return List.of( - m_detector.detect( - in, - params.cameraCalibrationCoefficients, - useNativePoseEst, - params.numIterations, - params.tagWidthMeters)); + public AprilTagDetectionPipe() { + super(); + + m_detector.addFamily("tag16h5"); + m_detector.addFamily("tag36h11"); } @Override - public void setParams(AprilTagDetectionPipeParams params) { - super.setParams(params); - m_detector.updateParams(params.detectorParams); + protected List process(Mat in) { + var ret = m_detector.detect(in); + + if (ret == null) { + return List.of(); + } + + return List.of(ret); + } + + @Override + public void setParams(AprilTagDetectionPipeParams newParams) { + if (this.params == null || !this.params.equals(newParams)) { + m_detector.setConfig(newParams.detectorParams); + + m_detector.clearFamilies(); + m_detector.addFamily(newParams.family.getNativeName()); + } + + super.setParams(newParams); } public void setNativePoseEstimationEnabled(boolean enabled) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java index ac2221421..f7c8ab2b2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java @@ -17,61 +17,37 @@ package org.photonvision.vision.pipe.impl; -import java.util.Objects; -import org.photonvision.vision.apriltag.AprilTagDetectorParams; +import edu.wpi.first.apriltag.AprilTagDetector; import org.photonvision.vision.apriltag.AprilTagFamily; -import org.photonvision.vision.calibration.CameraCalibrationCoefficients; public class AprilTagDetectionPipeParams { - public final AprilTagDetectorParams detectorParams; - public final CameraCalibrationCoefficients cameraCalibrationCoefficients; - public final int numIterations; - public final double tagWidthMeters; + public final AprilTagFamily family; + public final AprilTagDetector.Config detectorParams; - public AprilTagDetectionPipeParams( - AprilTagFamily tagFamily, - double decimate, - double blur, - int threads, - boolean debug, - boolean refineEdges, - int numIters, - double tagWidthMeters, - CameraCalibrationCoefficients cameraCalibrationCoefficients) { - detectorParams = - new AprilTagDetectorParams(tagFamily, decimate, blur, threads, debug, refineEdges); - this.cameraCalibrationCoefficients = cameraCalibrationCoefficients; - this.numIterations = numIters; - this.tagWidthMeters = tagWidthMeters; - } - - public AprilTagDetectionPipeParams( - AprilTagDetectorParams detectorParams, - CameraCalibrationCoefficients cameraCalibrationCoefficients, - int numIters, - double tagWidthMeters) { - this.detectorParams = detectorParams; - this.cameraCalibrationCoefficients = cameraCalibrationCoefficients; - this.numIterations = numIters; - this.tagWidthMeters = tagWidthMeters; + public AprilTagDetectionPipeParams(AprilTagFamily tagFamily, AprilTagDetector.Config config) { + this.family = tagFamily; + this.detectorParams = config; } @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - AprilTagDetectionPipeParams that = (AprilTagDetectionPipeParams) o; - return Objects.equals(detectorParams, that.detectorParams) - && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients); + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((family == null) ? 0 : family.hashCode()); + result = prime * result + ((detectorParams == null) ? 0 : detectorParams.hashCode()); + return result; } @Override - public String toString() { - return "AprilTagDetectionPipeParams{" - + "detectorParams=" - + detectorParams - + ", cameraCalibrationCoefficients=" - + cameraCalibrationCoefficients - + '}'; + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + AprilTagDetectionPipeParams other = (AprilTagDetectionPipeParams) obj; + if (family != other.family) return false; + if (detectorParams == null) { + if (other.detectorParams != null) return false; + } else if (!detectorParams.equals(other.detectorParams)) return false; + return true; } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java new file mode 100644 index 000000000..0d13493da --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -0,0 +1,89 @@ +/* + * 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 . + */ + +package org.photonvision.vision.pipe.impl; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import org.photonvision.vision.pipe.CVPipe; + +public class AprilTagPoseEstimatorPipe + extends CVPipe< + AprilTagDetection, + AprilTagPoseEstimate, + AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> { + private final AprilTagPoseEstimator m_poseEstimator = + new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0)); + + boolean useNativePoseEst; + + public AprilTagPoseEstimatorPipe() { + super(); + } + + @Override + protected AprilTagPoseEstimate process(AprilTagDetection in) { + return m_poseEstimator.estimateOrthogonalIteration(in, params.nIters); + } + + @Override + public void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams newParams) { + if (this.params == null || !this.params.equals(newParams)) { + m_poseEstimator.setConfig(newParams.config); + } + + super.setParams(newParams); + } + + public void setNativePoseEstimationEnabled(boolean enabled) { + this.useNativePoseEst = enabled; + } + + public static class AprilTagPoseEstimatorPipeParams { + final AprilTagPoseEstimator.Config config; + final int nIters; + + public AprilTagPoseEstimatorPipeParams(Config config, int nIters) { + this.config = config; + this.nIters = nIters; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((config == null) ? 0 : config.hashCode()); + result = prime * result + nIters; + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + AprilTagPoseEstimatorPipeParams other = (AprilTagPoseEstimatorPipeParams) obj; + if (config == null) { + if (other.config != null) return false; + } else if (!config.equals(other.config)) return false; + if (nIters != other.nIters) return false; + return true; + } + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index a1096c62c..4aa4594ae 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -17,7 +17,9 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.apriltag.jni.DetectionResult; +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; import java.util.ArrayList; @@ -25,12 +27,12 @@ import java.util.List; import org.opencv.core.Mat; import org.photonvision.common.util.math.MathUtils; import org.photonvision.raspi.PicamJNI; -import org.photonvision.vision.apriltag.AprilTagDetectorParams; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.pipe.CVPipe.CVPipeResult; import org.photonvision.vision.pipe.impl.*; +import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters; @@ -40,6 +42,7 @@ public class AprilTagPipeline extends CVPipeline targetList; - CVPipeResult> tagDetectionPipeResult; + CVPipeResult> tagDetectionPipeResult; // Use the solvePNP Enabled flag to enable native pose estimation aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled); @@ -144,16 +161,22 @@ public class AprilTagPipeline extends CVPipeline(); - for (DetectionResult detection : tagDetectionPipeResult.output) { + for (AprilTagDetection detection : tagDetectionPipeResult.output) { // TODO this should be in a pipe, not in the top level here (Matt) if (detection.getDecisionMargin() < settings.decisionMargin) continue; if (detection.getHamming() > settings.hammingDist) continue; + // Do pose estimation for all the tags that make it thru + // TODO + var poseResult = poseEstimatorPipe.run(detection); + sumPipeNanosElapsed += poseResult.nanosElapsed; + // populate the target list // Challenge here is that TrackedTarget functions with OpenCV Contour TrackedTarget target = new TrackedTarget( detection, + poseResult.output, new TargetCalculationParameters( false, null, null, null, null, frameStaticProperties)); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java index 12d53d28d..072faf074 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java @@ -25,7 +25,7 @@ import org.photonvision.vision.target.TargetModel; @JsonTypeName("AprilTagPipelineSettings") public class AprilTagPipelineSettings extends AdvancedPipelineSettings { public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11; - public double decimate = 1.0; + public int decimate = 1; public double blur = 0; public int threads = 1; public boolean debug = false; @@ -43,8 +43,6 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings { pipelineType = PipelineType.AprilTag; outputShowMultipleTargets = true; targetModel = TargetModel.k200mmAprilTag; - cameraExposure = -1; - cameraAutoExposure = true; ledMode = false; } diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 21d11fafd..1ffe37538 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -16,7 +16,8 @@ */ package org.photonvision.vision.target; -import edu.wpi.first.apriltag.jni.DetectionResult; +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; import edu.wpi.first.math.geometry.Transform3d; import java.util.HashMap; import java.util.List; @@ -64,24 +65,27 @@ public class TrackedTarget implements Releasable { calculateValues(params); } - public TrackedTarget(DetectionResult result, TargetCalculationParameters params) { - m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY()); + public TrackedTarget( + AprilTagDetection tagDetection, + AprilTagPoseEstimate tagPose, + TargetCalculationParameters params) { + m_targetOffsetPoint = new Point(tagDetection.getCenterX(), tagDetection.getCenterY()); m_robotOffsetPoint = new Point(); m_pitch = TargetCalculations.calculatePitch( - result.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength); + tagDetection.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength); m_yaw = TargetCalculations.calculateYaw( - result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength); + tagDetection.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength); var bestPose = new Transform3d(); var altPose = new Transform3d(); - if (result.getError1() <= result.getError2()) { - bestPose = result.getPoseResult1(); - altPose = result.getPoseResult2(); + if (tagPose.error1 <= tagPose.error2) { + bestPose = tagPose.pose1; + altPose = tagPose.pose2; } else { - bestPose = result.getPoseResult2(); - altPose = result.getPoseResult1(); + bestPose = tagPose.pose2; + altPose = tagPose.pose1; } bestPose = MathUtils.convertApriltagtoOpenCV(bestPose); @@ -90,7 +94,7 @@ public class TrackedTarget implements Releasable { m_bestCameraToTarget3d = bestPose; m_altCameraToTarget3d = altPose; - double[] corners = result.getCorners(); + double[] corners = tagDetection.getCorners(); Point[] cornerPoints = new Point[] { new Point(corners[0], corners[1]), @@ -103,7 +107,7 @@ public class TrackedTarget implements Releasable { m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints); m_mainContour = new Contour(contourMat); m_area = m_mainContour.getArea() / params.imageArea * 100; - m_fiducialId = result.getId(); + m_fiducialId = tagDetection.getId(); m_shape = null; // TODO implement skew? or just yeet @@ -125,7 +129,7 @@ public class TrackedTarget implements Releasable { MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec); setCameraRelativeRvec(rvec); - m_poseAmbiguity = result.getPoseAmbiguity(); + m_poseAmbiguity = tagPose.getAmbiguity(); } public void setFiducialId(int id) {