Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use refactored Apriltag API in WPILib (#644)

Browse Source 
Bumps to a wpilib dev version, until they cut a new release. Should help address the random NPEs from the old JNI.

Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
This commit is contained in:
Matt
2022-12-27 10:47:20 -08:00
committed by GitHub
parent 95c55f08cf
commit 2ebc27aa3b
14 changed files with 218 additions and 684 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
build.gradle
View File

@@ -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

2
photon-core/build.gradle
View File

@@ -23,6 +23,8 @@ dependencies {
// Zip
implementation 'org.zeroturnaround:zt-zip:1.14'
implementation wpilibTools.deps.wpilibJava("apriltag")
}
task writeCurrentVersionJava {

102
photon-core/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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<AprilTagJNI> 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);
}
}

246
photon-core/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java
View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> 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));
}
}

25
photon-core/src/main/java/org/photonvision/vision/apriltag/.styleguide-license
View File

@@ -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.
*/

103
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java
View File

@@ -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);
}
}

87
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetectorParams.java
View File

@@ -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
+ '}';
}
}

39
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagFamily.java
View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.apriltag;

43
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
View File

@@ -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<Mat, List<DetectionResult>, AprilTagDetectionPipeParams> {
extends CVPipe<Mat, List<AprilTagDetection>, AprilTagDetectionPipeParams> {
private final AprilTagDetector m_detector = new AprilTagDetector();
boolean useNativePoseEst;
@Override
protected List<DetectionResult> 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<AprilTagDetection> 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) {

68
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipeParams.java
View File

@@ -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;
}
}

89
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java Normal file
View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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;
}
}
}

61
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -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<CVPipelineResult, AprilTagPipel
private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
private final GrayscalePipe grayscalePipe = new GrayscalePipe();
private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
private final AprilTagPoseEstimatorPipe poseEstimatorPipe = new AprilTagPoseEstimatorPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
public AprilTagPipeline() {
@@ -65,15 +68,6 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
PicamJNI.setShouldCopyColor(true); // need the color image to grayscale
}
AprilTagDetectorParams aprilTagDetectionParams =
new AprilTagDetectorParams(
settings.tagFamily,
settings.decimate,
settings.blur,
settings.threads,
settings.debug,
settings.refineEdges);
// TODO (HACK): tag width is Fun because it really belongs in the "target model"
// We need the tag width for the JNI to figure out target pose, but we need a
// target model for the draw 3d targets pipeline to work...
@@ -102,12 +96,35 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
}
}
aprilTagDetectionPipe.setParams(
new AprilTagDetectionPipeParams(
aprilTagDetectionParams,
frameStaticProperties.cameraCalibration,
settings.numIterations,
tagWidth));
// AprilTagDetectorParams aprilTagDetectionParams =
// new AprilTagDetectorParams(
// settings.tagFamily,
// settings.decimate,
// settings.blur,
// settings.threads,
// settings.debug,
// settings.refineEdges);
var config = new AprilTagDetector.Config();
config.numThreads = settings.threads;
config.refineEdges = settings.refineEdges;
config.quadSigma = (float) settings.blur;
config.quadDecimate = settings.decimate;
aprilTagDetectionPipe.setParams(new AprilTagDetectionPipeParams(settings.tagFamily, config));
if (frameStaticProperties.cameraCalibration != null) {
var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
if (cameraMatrix != null) {
var cx = cameraMatrix.get(0, 2)[0];
var cy = cameraMatrix.get(1, 2)[0];
var fx = cameraMatrix.get(0, 0)[0];
var fy = cameraMatrix.get(1, 1)[0];
poseEstimatorPipe.setParams(
new AprilTagPoseEstimatorPipeParams(
new Config(tagWidth, fx, fy, cx, cy), settings.numIterations));
}
}
}
@Override
@@ -135,7 +152,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
var outputFrame = new Frame(new CVMat(grayscalePipeResult.output), frameStaticProperties);
List<TrackedTarget> targetList;
CVPipeResult<List<DetectionResult>> tagDetectionPipeResult;
CVPipeResult<List<AprilTagDetection>> tagDetectionPipeResult;
// Use the solvePNP Enabled flag to enable native pose estimation
aprilTagDetectionPipe.setNativePoseEstimationEnabled(settings.solvePNPEnabled);
@@ -144,16 +161,22 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
targetList = new ArrayList<>();
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));

4
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
View File

@@ -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;
}

30
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -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) {