process(CVMat in) {
+ var imgMat = in.getMat();
+ var detections = photonDetector.detect(imgMat);
+ // manually do corner refinement ourselves
+ if (params.useCornerRefinement) {
+ for (var detection : detections) {
+ double[] xCorners = detection.getXCorners();
+ double[] yCorners = detection.getYCorners();
+ Point[] cornerPoints =
+ new Point[] {
+ new Point(xCorners[0], yCorners[0]),
+ new Point(xCorners[1], yCorners[1]),
+ new Point(xCorners[2], yCorners[2]),
+ new Point(xCorners[3], yCorners[3])
+ };
+ double bltr =
+ Math.hypot(
+ cornerPoints[2].x - cornerPoints[0].x, cornerPoints[2].y - cornerPoints[0].y);
+ double brtl =
+ Math.hypot(
+ cornerPoints[3].x - cornerPoints[1].x, cornerPoints[3].y - cornerPoints[1].y);
+ double minDiag = Math.min(bltr, brtl);
+ int halfWindowLength =
+ (int) Math.ceil(kRefineWindowImageRatio * Math.min(imgMat.rows(), imgMat.cols()));
+ halfWindowLength += (int) (minDiag * kRefineWindowMarkerRatio);
+ // dont do refinement on small markers
+ if (halfWindowLength < 4) continue;
+ var halfWindowSize = new Size(halfWindowLength, halfWindowLength);
+ var ptsMat = new MatOfPoint2f(cornerPoints);
+ var criteria =
+ new TermCriteria(3, params.refinementMaxIterations, params.refinementMinErrorPx);
+ Imgproc.cornerSubPix(imgMat, ptsMat, halfWindowSize, new Size(-1, -1), criteria);
+ cornerPoints = ptsMat.toArray();
+ for (int i = 0; i < cornerPoints.length; i++) {
+ var pt = cornerPoints[i];
+ xCorners[i] = pt.x;
+ yCorners[i] = pt.y;
+ // If we want to debug the refinement window, draw a rectangle on the image
+ if (params.debugRefineWindow) {
+ drawCornerRefineWindow(imgMat, pt, halfWindowLength);
+ }
+ }
+ }
+ }
+ return List.of(detections);
}
@Override
- public void setParams(ArucoDetectionPipeParams params) {
- super.setParams(params);
+ public void setParams(ArucoDetectionPipeParams newParams) {
+ if (this.params == null || !this.params.equals(newParams)) {
+ photonDetector
+ .getDetector()
+ .setDictionary(Objdetect.getPredefinedDictionary(newParams.tagFamily));
+ var detectParams = photonDetector.getParams();
+
+ detectParams.set_adaptiveThreshWinSizeMin(newParams.threshMinSize);
+ detectParams.set_adaptiveThreshWinSizeStep(newParams.threshStepSize);
+ detectParams.set_adaptiveThreshWinSizeMax(newParams.threshMaxSize);
+ detectParams.set_adaptiveThreshConstant(newParams.threshConstant);
+
+ detectParams.set_errorCorrectionRate(newParams.errorCorrectionRate);
+
+ detectParams.set_useAruco3Detection(newParams.useAruco3);
+ detectParams.set_minSideLengthCanonicalImg(newParams.aruco3MinCanonicalImgSide);
+ detectParams.set_minMarkerLengthRatioOriginalImg((float) newParams.aruco3MinMarkerSideRatio);
+
+ photonDetector.setParams(detectParams);
+ }
+
+ super.setParams(newParams);
+ }
+
+ public PhotonArucoDetector getPhotonDetector() {
+ return photonDetector;
}
- public DetectorParameters getParameters() {
- return params == null ? null : params.detectorParams.getDetectorParameters();
+ private void drawCornerRefineWindow(Mat outputMat, Point corner, int windowSize) {
+ int thickness = (int) (Math.ceil(Math.max(outputMat.cols(), outputMat.rows()) * 0.003));
+ var pt1 = new Point(corner.x - windowSize, corner.y - windowSize);
+ var pt2 = new Point(corner.x + windowSize, corner.y + windowSize);
+ Imgproc.rectangle(outputMat, pt1, pt2, new Scalar(0, 0, 255), thickness);
}
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipeParams.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipeParams.java
index 62d3214030..34b1384e92 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipeParams.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipeParams.java
@@ -18,40 +18,90 @@
package org.photonvision.vision.pipe.impl;
import java.util.Objects;
-import org.opencv.objdetect.ArucoDetector;
-import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.opencv.objdetect.Objdetect;
+/** Detector parameters. See https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html. */
public class ArucoDetectionPipeParams {
- public ArucoDetector detectorParams;
- public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+ /** Tag family. Default: {@link Objdetect#DICT_APRILTAG_16h5}. */
+ public int tagFamily = Objdetect.DICT_APRILTAG_16h5;
- public ArucoDetectionPipeParams(
- ArucoDetector detector, CameraCalibrationCoefficients cameraCalibrationCoefficients) {
- this.detectorParams = detector;
- this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
- }
+ public int threshMinSize = 11;
+ public int threshStepSize = 40;
+ public int threshMaxSize = 91;
+ public double threshConstant = 10;
+
+ /**
+ * Bits allowed to be corrected, expressed as a ratio of the tag families theoretical maximum.
+ *
+ * E.g. 36h11 -> 11 * errorCorrectionRate = Max error bits
+ */
+ public double errorCorrectionRate = 0;
+
+ /**
+ * If obtained corners should be iteratively refined. This should always be on for 3D estimation.
+ */
+ public boolean useCornerRefinement = true;
+
+ /** Maximum corner refinement iterations. */
+ public int refinementMaxIterations = 30;
+
+ /**
+ * Minimum error (accuracy) for corner refinement in pixels. When a corner refinement iteration
+ * moves the corner by less than this value, the refinement is considered finished.
+ */
+ public double refinementMinErrorPx = 0.005;
+
+ public boolean debugRefineWindow = false;
+
+ /**
+ * If the 'Aruco3' speedup should be used. This is similar to AprilTag's 'decimate' value, but
+ * automatically determined with the given parameters.
+ *
+ *
T_i = aruco3MinMarkerSideRatio, and T_c = aruco3MinCanonicalImgSide
+ *
+ *
Scale factor = T_c / (T_c + T_i * max(img_width, img_height))
+ */
+ public boolean useAruco3 = false;
+
+ /** Minimum side length of markers expressed as a ratio of the largest image dimension. */
+ public double aruco3MinMarkerSideRatio = 0.02;
+
+ /** Minimum side length of the canonical image (marker after undoing perspective distortion). */
+ public int aruco3MinCanonicalImgSide = 32;
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
ArucoDetectionPipeParams that = (ArucoDetectionPipeParams) o;
- return Objects.equals(detectorParams, that.detectorParams)
- && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
+ return tagFamily == that.tagFamily
+ && threshMinSize == that.threshMinSize
+ && threshStepSize == that.threshStepSize
+ && threshMaxSize == that.threshMaxSize
+ && threshConstant == that.threshConstant
+ && errorCorrectionRate == that.errorCorrectionRate
+ && useCornerRefinement == that.useCornerRefinement
+ && refinementMaxIterations == that.refinementMaxIterations
+ && refinementMinErrorPx == that.refinementMinErrorPx
+ && useAruco3 == that.useAruco3
+ && aruco3MinMarkerSideRatio == that.aruco3MinMarkerSideRatio
+ && aruco3MinCanonicalImgSide == that.aruco3MinCanonicalImgSide;
}
@Override
public int hashCode() {
- return Objects.hash(detectorParams, cameraCalibrationCoefficients);
- }
-
- @Override
- public String toString() {
- return "ArucoDetectionPipeParams{"
- + "detectorParams="
- + detectorParams
- + ", cameraCalibrationCoefficients="
- + cameraCalibrationCoefficients
- + '}';
+ return Objects.hash(
+ tagFamily,
+ threshMinSize,
+ threshStepSize,
+ threshMaxSize,
+ threshConstant,
+ errorCorrectionRate,
+ useCornerRefinement,
+ refinementMaxIterations,
+ refinementMinErrorPx,
+ useAruco3,
+ aruco3MinMarkerSideRatio,
+ aruco3MinCanonicalImgSide);
}
}
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
new file mode 100644
index 0000000000..579996890e
--- /dev/null
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java
@@ -0,0 +1,140 @@
+/*
+ * 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.AprilTagPoseEstimate;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
+import org.opencv.core.Point3;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class ArucoPoseEstimatorPipe
+ extends CVPipe<
+ ArucoDetectionResult,
+ AprilTagPoseEstimate,
+ ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams> {
+ // image points of marker corners
+ private final MatOfPoint2f imagePoints = new MatOfPoint2f(Mat.zeros(4, 1, CvType.CV_32FC2));
+ // rvec/tvec estimations from solvepnp
+ private final List rvecs = new ArrayList<>();
+ private final List tvecs = new ArrayList<>();
+ // unused parameters
+ private final Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
+ private final Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
+ // reprojection error of solvepnp estimations
+ private final Mat reprojectionErrors = Mat.zeros(2, 1, CvType.CV_32F);
+
+ private final int kNaNRetries = 1;
+
+ private Translation3d tvecToTranslation3d(Mat mat) {
+ double[] tvec = new double[3];
+ mat.get(0, 0, tvec);
+ return new Translation3d(tvec[0], tvec[1], tvec[2]);
+ }
+
+ private Rotation3d rvecToRotation3d(Mat mat) {
+ double[] rvec = new double[3];
+ mat.get(0, 0, rvec);
+ return new Rotation3d(VecBuilder.fill(rvec[0], rvec[1], rvec[2]));
+ }
+
+ @Override
+ protected AprilTagPoseEstimate process(ArucoDetectionResult in) {
+ // We receive 2d corners as (BL, BR, TR, TL) but we want (BR, BL, TL, TR)
+ double[] xCorn = in.getXCorners();
+ double[] yCorn = in.getYCorners();
+ imagePoints.put(0, 0, new float[] {(float) xCorn[1], (float) yCorn[1]});
+ imagePoints.put(1, 0, new float[] {(float) xCorn[0], (float) yCorn[0]});
+ imagePoints.put(2, 0, new float[] {(float) xCorn[3], (float) yCorn[3]});
+ imagePoints.put(3, 0, new float[] {(float) xCorn[2], (float) yCorn[2]});
+
+ float[] reprojErrors = new float[2];
+ // very rarely the solvepnp solver returns NaN results, so we retry with slight noise added
+ for (int i = 0; i < kNaNRetries + 1; i++) {
+ // SolvePnP with SOLVEPNP_IPPE_SQUARE solver
+ Calib3d.solvePnPGeneric(
+ params.objectPoints,
+ imagePoints,
+ params.calibration.getCameraIntrinsicsMat(),
+ params.calibration.getDistCoeffsMat(),
+ rvecs,
+ tvecs,
+ false,
+ Calib3d.SOLVEPNP_IPPE_SQUARE,
+ rvec,
+ tvec,
+ reprojectionErrors);
+
+ // check if we got a NaN result
+ reprojectionErrors.get(0, 0, reprojErrors);
+ if (!Double.isNaN(reprojErrors[0])) break;
+ else { // add noise and retry
+ double[] br = imagePoints.get(0, 0);
+ br[0] -= 0.001;
+ br[1] -= 0.001;
+ imagePoints.put(0, 0, br);
+ }
+ }
+
+ // create AprilTagPoseEstimate with results
+ if (tvecs.isEmpty())
+ return new AprilTagPoseEstimate(new Transform3d(), new Transform3d(), 0, 0);
+ return new AprilTagPoseEstimate(
+ new Transform3d(tvecToTranslation3d(tvecs.get(0)), rvecToRotation3d(rvecs.get(0))),
+ new Transform3d(tvecToTranslation3d(tvecs.get(1)), rvecToRotation3d(rvecs.get(1))),
+ reprojErrors[0],
+ reprojErrors[1]);
+ }
+
+ @Override
+ public void setParams(ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams newParams) {
+ super.setParams(newParams);
+ }
+
+ 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/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
index d1837ff770..4a17673c94 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
@@ -31,6 +31,7 @@
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
@@ -69,27 +70,15 @@ protected void setPipeParamsImpl() {
// Sanitize thread count - not supported to have fewer than 1 threads
settings.threads = Math.max(1, settings.threads);
- // if (cameraQuirks.hasQuirk(CameraQuirk.PiCam) && LibCameraJNI.isSupported()) {
- // // TODO: Picam grayscale
- // LibCameraJNI.setRotation(settings.inputImageRotationMode.value);
- // // LibCameraJNI.setShouldCopyColor(true); // need the color image to grayscale
- // }
-
- // 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...
-
// for now, hard code tag width based on enum value
- double tagWidth = Units.inchesToMeters(3 * 2); // for 6in 16h5 tag.
-
- // AprilTagDetectorParams aprilTagDetectionParams =
- // new AprilTagDetectorParams(
- // settings.tagFamily,
- // settings.decimate,
- // settings.blur,
- // settings.threads,
- // settings.debug,
- // settings.refineEdges);
+ double tagWidth;
+ if (settings.tagFamily == AprilTagFamily.kTag36h11) {
+ // 2024 tag, guess 6.5in
+ tagWidth = Units.inchesToMeters(6.5);
+ } else {
+ // 2023/other: best guess is 6in
+ tagWidth = Units.inchesToMeters(6);
+ }
var config = new AprilTagDetector.Config();
config.numThreads = settings.threads;
@@ -125,8 +114,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
long sumPipeNanosElapsed = 0L;
if (frame.type != FrameThresholdType.GREYSCALE) {
- // TODO so all cameras should give us GREYSCALE -- how should we handle if not?
- // Right now, we just return nothing
+ // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
return new CVPipelineResult(0, 0, List.of(), frame);
}
@@ -193,20 +181,13 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
new Transform3d(
new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
// match expected AprilTag coordinate system
- // TODO cleanup coordinate systems in wpilib 2024
- var apriltagTrl =
- CoordinateSystem.convert(
- camToTag.getTranslation(), CoordinateSystem.NWU(), CoordinateSystem.EDN());
- var apriltagRot =
- CoordinateSystem.convert(
- new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
- .plus(
- CoordinateSystem.convert(
- camToTag.getRotation(),
- CoordinateSystem.NWU(),
- CoordinateSystem.EDN()));
- apriltagRot = new Rotation3d(0, Math.PI, 0).plus(apriltagRot);
- camToTag = new Transform3d(apriltagTrl, apriltagRot);
+ camToTag =
+ CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
+ // (AprilTag expects Z axis going into tag)
+ camToTag =
+ new Transform3d(
+ camToTag.getTranslation(),
+ new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation()));
tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
}
}
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 47faf82e29..6db514d005 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
@@ -32,7 +32,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
public int numIterations = 40;
public int hammingDist = 0;
public int decisionMargin = 35;
- public boolean doMultiTarget = true;
+ public boolean doMultiTarget = false;
public boolean doSingleTargetAlways = false;
// 3d settings
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 26a5c60151..4d80029cbd 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
@@ -34,29 +34,36 @@
package org.photonvision.vision.pipeline;
+import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.geometry.CoordinateSystem;
+import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.Mat;
+import org.opencv.imgproc.Imgproc;
+import org.opencv.objdetect.Objdetect;
+import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.vision.aruco.ArucoDetectionResult;
-import org.photonvision.vision.aruco.ArucoDetectorParams;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.*;
+import org.photonvision.vision.pipe.impl.ArucoPoseEstimatorPipe.ArucoPoseEstimatorPipeParams;
+import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipeParams;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TrackedTarget;
import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
public class ArucoPipeline extends CVPipeline {
- private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
- private final GrayscalePipe grayscalePipe = new GrayscalePipe();
-
private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
+ private final ArucoPoseEstimatorPipe singleTagPoseEstimatorPipe = new ArucoPoseEstimatorPipe();
+ private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
- ArucoDetectorParams m_arucoDetectorParams = new ArucoDetectorParams();
-
public ArucoPipeline() {
super(FrameThresholdType.GREYSCALE);
settings = new ArucoPipelineSettings();
@@ -69,59 +76,171 @@ public ArucoPipeline(ArucoPipelineSettings settings) {
@Override
protected void setPipeParamsImpl() {
- // Sanitize thread count - not supported to have fewer than 1 threads
- settings.threads = Math.max(1, settings.threads);
-
- RotateImagePipe.RotateImageParams rotateImageParams =
- new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
- rotateImagePipe.setParams(rotateImageParams);
-
- m_arucoDetectorParams.setDecimation((float) settings.decimate);
- m_arucoDetectorParams.setCornerRefinementMaxIterations(settings.numIterations);
- m_arucoDetectorParams.setCornerAccuracy(settings.cornerAccuracy);
+ var params = new ArucoDetectionPipeParams();
+ // sanitize and record settings
+
+ switch (settings.tagFamily) {
+ case kTag36h11:
+ params.tagFamily = Objdetect.DICT_APRILTAG_36h11;
+ break;
+ case kTag25h9:
+ params.tagFamily = Objdetect.DICT_APRILTAG_25h9;
+ break;
+ default:
+ params.tagFamily = Objdetect.DICT_APRILTAG_16h5;
+ }
- arucoDetectionPipe.setParams(
- new ArucoDetectionPipeParams(
- m_arucoDetectorParams.getDetector(), frameStaticProperties.cameraCalibration));
+ int threshMinSize = Math.max(3, settings.threshWinSizes.getFirst());
+ settings.threshWinSizes.setFirst(threshMinSize);
+ params.threshMinSize = threshMinSize;
+ int threshStepSize = Math.max(2, settings.threshStepSize);
+ settings.threshStepSize = threshStepSize;
+ params.threshStepSize = threshStepSize;
+ int threshMaxSize = Math.max(threshMinSize, settings.threshWinSizes.getSecond());
+ settings.threshWinSizes.setSecond(threshMaxSize);
+ params.threshMaxSize = threshMaxSize;
+ params.threshConstant = settings.threshConstant;
+
+ params.useCornerRefinement = settings.useCornerRefinement;
+ params.refinementMaxIterations = settings.refineNumIterations;
+ params.refinementMinErrorPx = settings.refineMinErrorPx;
+ params.useAruco3 = settings.useAruco3;
+ params.aruco3MinMarkerSideRatio = settings.aruco3MinMarkerSideRatio;
+ params.aruco3MinCanonicalImgSide = settings.aruco3MinCanonicalImgSide;
+ arucoDetectionPipe.setParams(params);
+
+ if (frameStaticProperties.cameraCalibration != null) {
+ var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
+ if (cameraMatrix != null && cameraMatrix.rows() > 0) {
+ var estimatorParams =
+ new ArucoPoseEstimatorPipeParams(
+ frameStaticProperties.cameraCalibration, Units.inchesToMeters(6));
+ singleTagPoseEstimatorPipe.setParams(estimatorParams);
+
+ // TODO global state ew
+ var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+ multiTagPNPPipe.setParams(
+ new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+ }
+ }
}
@Override
protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) {
long sumPipeNanosElapsed = 0L;
- Mat rawInputMat;
- rawInputMat = frame.colorImage.getMat();
- List targetList;
- CVPipeResult> tagDetectionPipeResult;
+ if (frame.type != FrameThresholdType.GREYSCALE) {
+ // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
+ return new CVPipelineResult(0, 0, List.of(), frame);
+ }
- if (rawInputMat.empty()) {
- return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), frame);
+ CVPipeResult> tagDetectionPipeResult;
+ tagDetectionPipeResult = arucoDetectionPipe.run(frame.processedImage);
+ sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
+
+ // If we want to debug the thresholding steps, draw the first step to the color image
+ if (settings.debugThreshold) {
+ drawThresholdFrame(
+ frame.processedImage.getMat(),
+ frame.colorImage.getMat(),
+ settings.threshWinSizes.getFirst(),
+ settings.threshConstant);
}
- tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
- targetList = new ArrayList<>();
+ List targetList = new ArrayList<>();
for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
- // TODO this should be in a pipe, not in the top level here (Matt)
-
- // populate the target list
- // Challenge here is that TrackedTarget functions with OpenCV Contour
+ // Populate target list for multitag
+ // (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should
+ // not be necessary.)
TrackedTarget target =
new TrackedTarget(
detection,
+ null,
new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties));
- var correctedBestPose = target.getBestCameraToTarget3d();
+ targetList.add(target);
+ }
- target.setBestCameraToTarget3d(
- new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+ // Do multi-tag pose estimation
+ MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
+ if (settings.solvePNPEnabled && settings.doMultiTarget) {
+ var multiTagOutput = multiTagPNPPipe.run(targetList);
+ sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
+ multiTagResult = multiTagOutput.output;
+ }
- targetList.add(target);
+ // Do single-tag pose estimation
+ if (settings.solvePNPEnabled) {
+ // Clear target list that was used for multitag so we can add target transforms
+ targetList.clear();
+ // TODO global state again ew
+ var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
+
+ for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+ AprilTagPoseEstimate tagPoseEstimate = null;
+ // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
+ if (settings.doSingleTargetAlways
+ || !multiTagResult.fiducialIDsUsed.contains(detection.getId())) {
+ var poseResult = singleTagPoseEstimatorPipe.run(detection);
+ sumPipeNanosElapsed += poseResult.nanosElapsed;
+ tagPoseEstimate = poseResult.output;
+ }
+
+ // If single-tag estimation was not done, this is a multi-target tag from the layout
+ if (tagPoseEstimate == null) {
+ // compute this tag's camera-to-tag transform using the multitag result
+ var tagPose = atfl.getTagPose(detection.getId());
+ if (tagPose.isPresent()) {
+ var camToTag =
+ new Transform3d(
+ new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
+ // match expected OpenCV coordinate system
+ camToTag =
+ CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
+
+ tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
+ }
+ }
+
+ // populate the target list
+ // Challenge here is that TrackedTarget functions with OpenCV Contour
+ TrackedTarget target =
+ new TrackedTarget(
+ detection,
+ tagPoseEstimate,
+ new TargetCalculationParameters(
+ false, null, null, null, null, frameStaticProperties));
+
+ var correctedBestPose =
+ MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
+ var correctedAltPose =
+ MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
+
+ target.setBestCameraToTarget3d(
+ new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+ target.setAltCameraToTarget3d(
+ new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
+
+ targetList.add(target);
+ }
}
var fpsResult = calculateFPSPipe.run(null);
var fps = fpsResult.output;
- return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
+ return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+ }
+
+ private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) {
+ if (windowSize % 2 == 0) windowSize++;
+ Imgproc.adaptiveThreshold(
+ greyMat,
+ outputMat,
+ 255,
+ Imgproc.ADAPTIVE_THRESH_MEAN_C,
+ Imgproc.THRESH_BINARY_INV,
+ windowSize,
+ constant);
}
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java
index 265a8d202e..94eb963435 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java
@@ -18,23 +18,35 @@
package org.photonvision.vision.pipeline;
import com.fasterxml.jackson.annotation.JsonTypeName;
+import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.target.TargetModel;
@JsonTypeName("ArucoPipelineSettings")
public class ArucoPipelineSettings extends AdvancedPipelineSettings {
- public double decimate = 1;
- public int threads = 2;
- public int numIterations = 100;
- public double cornerAccuracy = 25.0;
- public boolean useAruco3 = true;
+ public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5;
- // 3d settings
+ public IntegerCouple threshWinSizes = new IntegerCouple(11, 91);
+ public int threshStepSize = 40;
+ public double threshConstant = 10;
+ public boolean debugThreshold = false;
+
+ public boolean useCornerRefinement = true;
+ public int refineNumIterations = 30;
+ public double refineMinErrorPx = 0.005;
+
+ public boolean useAruco3 = false;
+ public double aruco3MinMarkerSideRatio = 0.02;
+ public int aruco3MinCanonicalImgSide = 32;
+
+ public boolean doMultiTarget = false;
+ public boolean doSingleTargetAlways = false;
public ArucoPipelineSettings() {
super();
pipelineType = PipelineType.Aruco;
outputShowMultipleTargets = true;
- targetModel = TargetModel.kAruco6in_16h5;
+ targetModel = TargetModel.k6in_16h5;
cameraExposure = -1;
cameraAutoExposure = true;
ledMode = false;
diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
index 198cbd5139..3e8a967ba2 100644
--- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
+++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
@@ -114,20 +114,29 @@ public enum TargetModel implements Releasable {
new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0),
new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
Units.inchesToMeters(3.25 * 2)),
- kAruco6in_16h5( // Corners of the tag's inner black square (excluding white border)
+ k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
+ // do not
List.of(
new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
- new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+ new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)),
Units.inchesToMeters(3 * 2)),
- k6in_16h5( // Corners of the tag's inner black square (excluding white border)
+ // 2024 FRC tag. 6.5in inner tag, 8.125 overall
+ kAruco6p5in_36h11(
List.of(
- new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
- new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
- new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
- new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)),
- Units.inchesToMeters(3 * 2));
+ new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0),
+ new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0),
+ new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0),
+ new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)),
+ Units.inchesToMeters(6.5)),
+ k6p5in_36h11(
+ List.of(
+ new Point3(-Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0),
+ new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0),
+ new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0),
+ new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)),
+ Units.inchesToMeters(6.5));
@JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;
@JsonIgnore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();
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 9be1e9162c..2a28d05e56 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
@@ -18,10 +18,7 @@
import edu.wpi.first.apriltag.AprilTagDetection;
import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
@@ -147,25 +144,12 @@ public TrackedTarget(
// TODO implement skew? or just yeet
m_skew = 0;
-
- var tvec = new Mat(3, 1, CvType.CV_64FC1);
- tvec.put(
- 0,
- 0,
- new double[] {
- bestPose.getTranslation().getX(),
- bestPose.getTranslation().getY(),
- bestPose.getTranslation().getZ()
- });
- setCameraRelativeTvec(tvec);
-
- // Opencv expects a 3d vector with norm = angle and direction = axis
- var rvec = new Mat(3, 1, CvType.CV_64FC1);
- MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
- setCameraRelativeRvec(rvec);
}
- public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters params) {
+ public TrackedTarget(
+ ArucoDetectionResult result,
+ AprilTagPoseEstimate tagPose,
+ TargetCalculationParameters params) {
m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
m_robotOffsetPoint = new Point();
var yawPitch =
@@ -179,8 +163,8 @@ public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters pa
m_yaw = yawPitch.getFirst();
m_pitch = yawPitch.getSecond();
- double[] xCorners = result.getxCorners();
- double[] yCorners = result.getyCorners();
+ double[] xCorners = result.getXCorners();
+ double[] yCorners = result.getYCorners();
Point[] cornerPoints =
new Point[] {
@@ -198,25 +182,38 @@ public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters pa
m_shape = null;
// TODO implement skew? or just yeet
+ m_skew = 0;
- var tvec = new Mat(3, 1, CvType.CV_64FC1);
- tvec.put(0, 0, result.getTvec());
- setCameraRelativeTvec(tvec);
+ var bestPose = new Transform3d();
+ var altPose = new Transform3d();
+ if (tagPose != null) {
+ if (tagPose.error1 <= tagPose.error2) {
+ bestPose = tagPose.pose1;
+ altPose = tagPose.pose2;
+ } else {
+ bestPose = tagPose.pose2;
+ altPose = tagPose.pose1;
+ }
- var rvec = new Mat(3, 1, CvType.CV_64FC1);
- rvec.put(0, 0, result.getRvec());
- setCameraRelativeRvec(rvec);
+ m_bestCameraToTarget3d = bestPose;
+ m_altCameraToTarget3d = altPose;
+
+ m_poseAmbiguity = tagPose.getAmbiguity();
- {
- Translation3d translation =
- // new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
- new Translation3d(result.getTvec()[0], result.getTvec()[1], result.getTvec()[2]);
- var axisangle =
- VecBuilder.fill(result.getRvec()[0], result.getRvec()[1], result.getRvec()[2]);
- Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+ var tvec = new Mat(3, 1, CvType.CV_64FC1);
+ tvec.put(
+ 0,
+ 0,
+ new double[] {
+ bestPose.getTranslation().getX(),
+ bestPose.getTranslation().getY(),
+ bestPose.getTranslation().getZ()
+ });
+ setCameraRelativeTvec(tvec);
- m_bestCameraToTarget3d =
- MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
+ var rvec = new Mat(3, 1, CvType.CV_64FC1);
+ MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
+ setCameraRelativeRvec(rvec);
}
}
@@ -410,8 +407,8 @@ public static List simpleFromTrackedTargets(List> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
index 1c9ef2ce1e..b2c7317c32 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -24,6 +24,8 @@
package org.photonvision.simulation;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.cscore.VideoMode.PixelFormat;
@@ -31,12 +33,12 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.util.RuntimeLoader;
+import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
+import java.util.stream.Collectors;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
@@ -49,9 +51,12 @@
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
+import org.photonvision.estimation.CameraTargetRelation;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
+import org.photonvision.estimation.VisionEstimation;
+import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -77,6 +82,8 @@ public class PhotonCameraSim implements AutoCloseable {
private double minTargetAreaPercent;
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
+ private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+
// video stream simulation
private final CvSource videoSimRaw;
private final Mat videoSimFrameRaw = new Mat();
@@ -89,10 +96,7 @@ public class PhotonCameraSim implements AutoCloseable {
static {
try {
- var loader =
- new RuntimeLoader<>(
- Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
- loader.loadLibrary();
+ CombinedRuntimeLoader.loadLibraries(OpenCVHelp.class, Core.NATIVE_LIBRARY_NAME, "cscorejni");
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
@@ -205,23 +209,16 @@ public Mat getVideoSimFrameRaw() {
* @return If this vision target can be seen before image projection.
*/
public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) {
- // var rel = new CameraTargetRelation(camPose, target.getPose());
- // TODO: removal awaiting wpilib Rotation3d performance improvements
- var relTarget = RotTrlTransform3d.makeRelativeTo(camPose).apply(target.getPose());
- var camToTargYaw = new Rotation2d(relTarget.getX(), relTarget.getY());
- var camToTargPitch =
- new Rotation2d(Math.hypot(relTarget.getX(), relTarget.getY()), -relTarget.getZ());
- var relCam = RotTrlTransform3d.makeRelativeTo(target.getPose()).apply(camPose);
- var targToCamAngle = new Rotation2d(relCam.getX(), Math.hypot(relCam.getY(), relCam.getZ()));
+ var rel = new CameraTargetRelation(camPose, target.getPose());
return (
// target translation is outside of camera's FOV
- (Math.abs(camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
- && (Math.abs(camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
+ (Math.abs(rel.camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
+ && (Math.abs(rel.camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
&& (!target.getModel().isPlanar
- || Math.abs(targToCamAngle.getDegrees())
+ || Math.abs(rel.targToCamAngle.getDegrees())
< 90) // camera is behind planar target and it should be occluded
- && (relTarget.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
+ && (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
}
/**
@@ -524,11 +521,27 @@ public PhotonPipelineResult process(
videoSimProcessed.putFrame(videoSimFrameProcessed);
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
- // put this simulated data to NT
+ // calculate multitag results
+ var multitagResults = new MultiTargetPNPResults();
+ // TODO: Implement ATFL subscribing in backend
+ // var tagLayout = cam.getAprilTagFieldLayout();
+ var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
+ if (visibleLayoutTags.size() > 1) {
+ List usedIDs =
+ visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
+ var pnpResults =
+ VisionEstimation.estimateCamPosePNP(
+ prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout);
+ multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs);
+ }
+
+ // sort target order
if (sortMode != null) {
detectableTgts.sort(sortMode.getComparator());
}
- return new PhotonPipelineResult(latencyMillis, detectableTgts);
+
+ // put this simulated data to NT
+ return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults);
}
/**
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
index 7a32e413a3..cc9ead27fb 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
+++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
@@ -28,7 +28,7 @@
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.RuntimeLoader;
+import edu.wpi.first.util.CombinedRuntimeLoader;
import java.awt.image.BufferedImage;
import java.io.IOException;
import java.util.ArrayList;
@@ -68,10 +68,7 @@ public class VideoSimUtil {
static {
try {
- var loader =
- new RuntimeLoader<>(
- Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
- loader.loadLibrary();
+ CombinedRuntimeLoader.loadLibraries(OpenCVHelp.class, Core.NATIVE_LIBRARY_NAME, "cscorejni");
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java
index e483833111..3cab3d496e 100644
--- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java
+++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java
@@ -40,7 +40,6 @@
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
-import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.List;
import org.junit.jupiter.api.BeforeAll;
@@ -101,13 +100,9 @@ public static void setUp() {
"ntcorejni",
"wpinetjni",
"wpiHaljni",
+ Core.NATIVE_LIBRARY_NAME,
"cscorejni",
- "cscorejnicvstatic");
-
- var loader =
- new RuntimeLoader<>(
- Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
- loader.loadLibrary();
+ "apriltagjni");
} catch (Exception e) {
e.printStackTrace();
}
diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
index c53e006717..d9ed86d8ed 100644
--- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
+++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
@@ -46,7 +46,6 @@
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
-import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.ArrayList;
import java.util.List;
@@ -101,13 +100,9 @@ public static void setUp() {
"ntcorejni",
"wpinetjni",
"wpiHaljni",
+ Core.NATIVE_LIBRARY_NAME,
"cscorejni",
- "cscorejnicvstatic");
-
- var loader =
- new RuntimeLoader<>(
- Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
- loader.loadLibrary();
+ "apriltagjni");
} catch (Exception e) {
e.printStackTrace();
}
diff --git a/photon-server/build.gradle b/photon-server/build.gradle
index 92b50a81b8..2fdfcff4ec 100644
--- a/photon-server/build.gradle
+++ b/photon-server/build.gradle
@@ -1,11 +1,11 @@
plugins {
- id 'edu.wpi.first.WpilibTools' version '1.1.0'
+ id "application"
+ id 'com.github.johnrengelman.shadow' version '8.1.1'
+ id "com.github.node-gradle.node" version "7.0.1"
+ id "org.hidetake.ssh" version "2.11.2"
+ id 'edu.wpi.first.WpilibTools' version '1.3.0'
}
-apply plugin: "application"
-apply plugin: "com.github.johnrengelman.shadow"
-apply plugin: "org.hidetake.ssh"
-
evaluationDependsOn(':photon-core')
mainClassName = 'org.photonvision.Main'
@@ -15,26 +15,12 @@ version versionString + (project.hasProperty('pionly') ? "-raspi" : "")
apply from: "${rootDir}/shared/common.gradle"
-def nativeConfigName = 'wpilibTestNative'
-def nativeConfig = configurations.create(nativeConfigName)
-
-def nativeTasks = wpilibTools.createExtractionTasks { configurationName = nativeConfigName }
-
-nativeTasks.addToSourceSetResources(sourceSets.main)
-
-nativeConfig.dependencies.add wpilibTools.deps.cscore()
-nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
-nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
-nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
-nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
-nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
-nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
-
dependencies {
implementation project(':photon-core')
implementation project(':photon-targeting')
implementation "io.javalin:javalin:$javalinVersion"
+ implementation "org.slf4j:slf4j-simple:2.0.7"
implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("wpimath")
@@ -42,50 +28,44 @@ dependencies {
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("wpilibj")
+ implementation wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
implementation "org.msgpack:msgpack-core:0.9.0"
implementation "org.msgpack:jackson-dataformat-msgpack:0.9.0"
-
- implementation "org.slf4j:slf4j-simple:2.0.7"
}
shadowJar {
- configurations = [project.configurations.runtimeClasspath]
- String name = "photonvision-${project.version}"
- archiveClassifier.set(wpilibTools.platformMapper.currentPlatform.platformName)
archiveBaseName = "photonvision"
- archiveVersion = project.version
- // archiveFileName.set("${name}.jar")
+ archiveVersion = project.version as String
+ archiveClassifier.set(wpilibTools.currentPlatform.platformName)
+ configurations = [
+ project.configurations.runtimeClasspath
+ ]
}
-task runNpmOnClient(type: Exec) {
- workingDir "${projectDir}/../photon-client"
- if (System.getProperty('os.name').toLowerCase(Locale.ROOT).contains('windows')) {
- commandLine 'cmd', '/c', 'npm run build'
- } else {
- commandLine 'npm', 'run', 'build'
- }
+
+node {
+ nodeProjectDir = file("${projectDir}/../photon-client")
}
-task copyClientUIToResources(type: Copy) {
+tasks.register('copyClientUIToResources', Copy) {
from "${projectDir}/../photon-client/dist/"
into "${projectDir}/src/main/resources/web/"
}
-task buildAndCopyUI {}
-
-buildAndCopyUI.dependsOn copyClientUIToResources
-copyClientUIToResources.dependsOn runNpmOnClient
-copyClientUIToResources.shouldRunAfter runNpmOnClient
+tasks.register("buildAndCopyUI") {
+ dependsOn "npm_run_build"
+ finalizedBy "copyClientUIToResources"
+}
run {
if (project.hasProperty("profile")) {
jvmArgs=[
- "-Dcom.sun.management.jmxremote=true",
- "-Dcom.sun.management.jmxremote.ssl=false",
- "-Dcom.sun.management.jmxremote.authenticate=false",
- "-Dcom.sun.management.jmxremote.port=5000",
- "-Djava.rmi.server.hostname=0.0.0.0",
+ "-Dcom.sun.management.jmxremote=true",
+ "-Dcom.sun.management.jmxremote.ssl=false",
+ "-Dcom.sun.management.jmxremote.authenticate=false",
+ "-Dcom.sun.management.jmxremote.port=5000",
+ "-Djava.rmi.server.hostname=0.0.0.0",
]
}
}
@@ -106,9 +86,6 @@ remotes {
}
}
-import java.io.*;
-import java.net.*;
-
task findDeployTarget {
doLast {
if(project.hasProperty('tgtIP')){
@@ -143,23 +120,14 @@ task findDeployTarget {
}
run {
- environment "PATH_PREFIX", "../"
+ environment "PATH_PREFIX", "../"
}
-// task overrideToPi {
-// doLast {
-// project.setProperty('ArchOverride', 'linuxarm32')
-// }
-// }
-
-
-
task deploy {
- //dependsOn overrideToPi
- dependsOn assemble
dependsOn findDeployTarget
+ dependsOn assemble
- doLast {
+ doLast {
println 'Starting deployment to ' + findDeployTarget.rmt.host
println 'targetArch = ' + wpilibTools.platformMapper.currentPlatform.platformName
ssh.run{
@@ -170,7 +138,7 @@ task deploy {
execute 'sleep 3'
// Copy into a folder owned by PI. Mostly because, as far as I can tell, the put command doesn't support sudo.
put from: "${projectDir}/build/libs/photonvision-${project.version}-${wpilibTools.platformMapper.currentPlatform.platformName}.jar", into: "/tmp/photonvision.jar"
- //belt-and-suspenders. Make sure the old jar is gone first.
+ //belt-and-suspenders. Make sure the old jar is gone first.
execute 'sudo rm -f /opt/photonvision/photonvision.jar'
//Copy in the new .jar and make sure it's executable
execute 'sudo mv /tmp/photonvision.jar /opt/photonvision/photonvision.jar'
diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle
index e4cc2d7d96..256b66ce70 100644
--- a/photon-targeting/build.gradle
+++ b/photon-targeting/build.gradle
@@ -1,11 +1,16 @@
-apply plugin: "java"
+plugins {
+ id "java"
+ id 'edu.wpi.first.WpilibTools' version '1.3.0'
+}
apply from: "${rootDir}/shared/common.gradle"
dependencies {
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("apriltag")
- implementation "org.ejml:ejml-simple:0.42"
+ implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+
+ implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
// Junit
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
index 3f89918d62..1f3b8b9761 100644
--- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
@@ -26,7 +26,7 @@
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.*;
-import edu.wpi.first.util.RuntimeLoader;
+import edu.wpi.first.util.CombinedRuntimeLoader;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
@@ -49,31 +49,22 @@
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
- private static RotTrlTransform3d NWU_TO_EDN;
- private static RotTrlTransform3d EDN_TO_NWU;
+ private static Rotation3d NWU_TO_EDN;
+ private static Rotation3d EDN_TO_NWU;
static {
try {
- var loader =
- new RuntimeLoader<>(
- Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
- loader.loadLibrary();
+ CombinedRuntimeLoader.loadLibraries(OpenCVHelp.class, Core.NATIVE_LIBRARY_NAME, "cscorejni");
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
- NWU_TO_EDN =
- new RotTrlTransform3d(
- new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)),
- new Translation3d());
- EDN_TO_NWU =
- new RotTrlTransform3d(
- new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)),
- new Translation3d());
+ NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0));
+ EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0));
}
public static Mat matrixToMat(SimpleMatrix matrix) {
- var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
+ var mat = new Mat(matrix.getNumRows(), matrix.getNumCols(), CvType.CV_64F);
mat.put(0, 0, matrix.getDDRM().getData());
return mat;
}
@@ -222,14 +213,12 @@ public static List reorderCircular(List elements, boolean backwards, i
return reordered;
}
- // TODO: RotTrlTransform3d removal awaiting Rotation3d performance improvements
/**
* Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
- return new RotTrlTransform3d(EDN_TO_NWU.apply(rot), new Translation3d())
- .apply(EDN_TO_NWU.inverse().getRotation());
+ return EDN_TO_NWU.unaryMinus().plus(rot.plus(EDN_TO_NWU));
}
/**
@@ -237,8 +226,7 @@ private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
- return new RotTrlTransform3d(NWU_TO_EDN.apply(rot), new Translation3d())
- .apply(NWU_TO_EDN.inverse().getRotation());
+ return NWU_TO_EDN.unaryMinus().plus(rot.plus(NWU_TO_EDN));
}
/**
@@ -246,7 +234,7 @@ private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Translation3d translationEDNtoNWU(Translation3d trl) {
- return EDN_TO_NWU.apply(trl);
+ return trl.rotateBy(EDN_TO_NWU);
}
/**
@@ -254,7 +242,7 @@ private static Translation3d translationEDNtoNWU(Translation3d trl) {
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Translation3d translationNWUtoEDN(Translation3d trl) {
- return NWU_TO_EDN.apply(trl);
+ return trl.rotateBy(NWU_TO_EDN);
}
/**
diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
index 2b79e920d6..11bd52ff9c 100644
--- a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
+++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
@@ -18,7 +18,6 @@
package org.photonvision.estimation;
import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
@@ -31,11 +30,6 @@
public class RotTrlTransform3d {
private final Translation3d trl;
private final Rotation3d rot;
- // TODO: removal awaiting wpilib Rotation3d performance improvements
- private double m_w;
- private double m_x;
- private double m_y;
- private double m_z;
/**
* A rotation-translation transformation.
@@ -48,25 +42,12 @@ public class RotTrlTransform3d {
*/
public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
this.rot = rot;
- var quat = rot.getQuaternion();
- m_w = quat.getW();
- m_x = quat.getX();
- m_y = quat.getY();
- m_z = quat.getZ();
this.trl = trl;
}
public RotTrlTransform3d(Pose3d initial, Pose3d last) {
- // this.rot = last.getRotation().minus(initial.getRotation());
- // this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot));
-
- var quat = initial.getRotation().getQuaternion();
- m_w = quat.getW();
- m_x = quat.getX();
- m_y = quat.getY();
- m_z = quat.getZ();
- this.rot = invrotate(last.getRotation());
- this.trl = last.getTranslation().minus(rotate(initial.getTranslation()));
+ this.rot = last.getRotation().minus(initial.getRotation());
+ this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot));
}
/**
@@ -85,38 +66,6 @@ public RotTrlTransform3d() {
this(new Rotation3d(), new Translation3d());
}
- private Translation3d rotate(Translation3d otrl) {
- final var p = new Quaternion(0.0, otrl.getX(), otrl.getY(), otrl.getZ());
- final var qprime = times(times(p), new Quaternion(m_w, -m_x, -m_y, -m_z));
- return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
- }
-
- private Translation3d invrotate(Translation3d otrl) {
- m_x = -m_x;
- m_y = -m_y;
- m_z = -m_z;
- var result = rotate(otrl);
- m_x = -m_x;
- m_y = -m_y;
- m_z = -m_z;
- return result;
- }
-
- private Rotation3d rotate(Rotation3d orot) {
- return new Rotation3d(times(orot.getQuaternion()));
- }
-
- private Rotation3d invrotate(Rotation3d orot) {
- m_x = -m_x;
- m_y = -m_y;
- m_z = -m_z;
- var result = rotate(orot);
- m_x = -m_x;
- m_y = -m_y;
- m_z = -m_z;
- return result;
- }
-
/**
* The rotation-translation transformation that makes poses in the world consider this pose as the
* new origin, or change the basis to this pose.
@@ -129,12 +78,9 @@ public static RotTrlTransform3d makeRelativeTo(Pose3d pose) {
/** The inverse of this transformation. Applying the inverse will "undo" this transformation. */
public RotTrlTransform3d inverse() {
- // var inverseRot = rot.unaryMinus();
- // var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
- // return new RotTrlTransform3d(inverseRot, inverseTrl);
-
- var inverseTrl = invrotate(trl).unaryMinus();
- return new RotTrlTransform3d(new Rotation3d(new Quaternion(m_w, -m_x, -m_y, -m_z)), inverseTrl);
+ var inverseRot = rot.unaryMinus();
+ var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
+ return new RotTrlTransform3d(inverseRot, inverseTrl);
}
/** This transformation as a Transform3d (as if of the origin) */
@@ -153,8 +99,7 @@ public Rotation3d getRotation() {
}
public Translation3d apply(Translation3d trl) {
- // return trl.rotateBy(rot).plus(this.trl);
- return rotate(trl).plus(this.trl);
+ return trl.rotateBy(rot).plus(this.trl);
}
public List applyTrls(List trls) {
@@ -162,7 +107,7 @@ public List applyTrls(List trls) {
}
public Rotation3d apply(Rotation3d rot) {
- return rotate(rot);
+ return rot.plus(this.rot);
}
public List applyRots(List rots) {
@@ -170,57 +115,10 @@ public List applyRots(List rots) {
}
public Pose3d apply(Pose3d pose) {
- // return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl),
- // pose.getRotation().plus(rot));
return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation()));
}
public List applyPoses(List poses) {
return poses.stream().map(this::apply).collect(Collectors.toList());
}
-
- // TODO: removal awaiting wpilib Rotation3d performance improvements
- private Quaternion times(Quaternion other) {
- final double o_w = other.getW();
- final double o_x = other.getX();
- final double o_y = other.getY();
- final double o_z = other.getZ();
- return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
- }
-
- private static Quaternion times(Quaternion a, Quaternion b) {
- final double m_w = a.getW();
- final double m_x = a.getX();
- final double m_y = a.getY();
- final double m_z = a.getZ();
- final double o_w = b.getW();
- final double o_x = b.getX();
- final double o_y = b.getY();
- final double o_z = b.getZ();
- return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
- }
-
- private static Quaternion times(
- double m_w,
- double m_x,
- double m_y,
- double m_z,
- double o_w,
- double o_x,
- double o_y,
- double o_z) {
- // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
-
- // v₁ x v₂
- final double cross_x = m_y * o_z - o_y * m_z;
- final double cross_y = o_x * m_z - m_x * o_z;
- final double cross_z = m_x * o_y - o_x * m_y;
-
- // v = w₁v₂ + w₂v₁ + v₁ x v₂
- final double new_x = o_x * m_w + (m_x * o_w) + cross_x;
- final double new_y = o_y * m_w + (m_y * o_w) + cross_y;
- final double new_z = o_z * m_w + (m_z * o_w) + cross_z;
-
- return new Quaternion(m_w * o_w - (m_x * o_x + m_y * o_y + m_z * o_z), new_x, new_y, new_z);
- }
}
diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle
index ecedfafb40..c1c32d0284 100644
--- a/photonlib-cpp-examples/aimandrange/build.gradle
+++ b/photonlib-cpp-examples/aimandrange/build.gradle
@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
id "com.dorongold.task-tree" version "2.1.0"
}
diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle
index ecedfafb40..c1c32d0284 100644
--- a/photonlib-cpp-examples/aimattarget/build.gradle
+++ b/photonlib-cpp-examples/aimattarget/build.gradle
@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
id "com.dorongold.task-tree" version "2.1.0"
}
diff --git a/photonlib-cpp-examples/apriltagExample/build.gradle b/photonlib-cpp-examples/apriltagExample/build.gradle
index ecedfafb40..c1c32d0284 100644
--- a/photonlib-cpp-examples/apriltagExample/build.gradle
+++ b/photonlib-cpp-examples/apriltagExample/build.gradle
@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
id "com.dorongold.task-tree" version "2.1.0"
}
diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle
index 0d262a1504..f86d65a4ae 100644
--- a/photonlib-cpp-examples/build.gradle
+++ b/photonlib-cpp-examples/build.gradle
@@ -1,5 +1,6 @@
plugins {
id "com.diffplug.spotless" version "6.1.2"
+ id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" apply false
}
allprojects {
diff --git a/photonlib-cpp-examples/examples.gradle b/photonlib-cpp-examples/examples.gradle
index 976440f33f..1207d05f1d 100644
--- a/photonlib-cpp-examples/examples.gradle
+++ b/photonlib-cpp-examples/examples.gradle
@@ -4,11 +4,11 @@ def EXCLUDED_DIRS = ["bin", "build"]
// List all non-hidden directories not in EXCUDED_DIRS
ext.exampleFolderNames = file("${rootDir}")
- .listFiles()
- .findAll {
- return (it.isDirectory()
- && !it.isHidden()
- && !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
- && it.toPath().resolve("build.gradle").toFile().exists())
- }
- .collect { it.name }
+ .listFiles()
+ .findAll {
+ return (it.isDirectory()
+ && !it.isHidden()
+ && !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
+ && it.toPath().resolve("build.gradle").toFile().exists())
+ }
+ .collect { it.name }
diff --git a/photonlib-cpp-examples/getinrange/build.gradle b/photonlib-cpp-examples/getinrange/build.gradle
index ecedfafb40..c1c32d0284 100644
--- a/photonlib-cpp-examples/getinrange/build.gradle
+++ b/photonlib-cpp-examples/getinrange/build.gradle
@@ -1,7 +1,7 @@
plugins {
id "cpp"
id "google-test-test-suite"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
id "com.dorongold.task-tree" version "2.1.0"
}
diff --git a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties
index 7f959b087a..24c4386e93 100644
--- a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties
+++ b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
-distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip
+distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle
index 7d6e58895a..f3750f10b2 100644
--- a/photonlib-java-examples/aimandrange/build.gradle
+++ b/photonlib-java-examples/aimandrange/build.gradle
@@ -1,6 +1,6 @@
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
}
sourceCompatibility = JavaVersion.VERSION_11
@@ -78,7 +78,11 @@ wpi.sim.addDriverstation()
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
- from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle
index d98e56b952..d0f6ecfecc 100644
--- a/photonlib-java-examples/aimattarget/build.gradle
+++ b/photonlib-java-examples/aimattarget/build.gradle
@@ -1,6 +1,6 @@
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
}
sourceCompatibility = JavaVersion.VERSION_11
@@ -78,7 +78,11 @@ wpi.sim.addDriverstation()
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
- from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle
index 35f533e690..68dd2983a2 100644
--- a/photonlib-java-examples/build.gradle
+++ b/photonlib-java-examples/build.gradle
@@ -1,5 +1,6 @@
plugins {
id "com.diffplug.spotless" version "6.1.2"
+ id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" apply false
}
apply from: "examples.gradle"
diff --git a/photonlib-java-examples/examples.gradle b/photonlib-java-examples/examples.gradle
index 976440f33f..1207d05f1d 100644
--- a/photonlib-java-examples/examples.gradle
+++ b/photonlib-java-examples/examples.gradle
@@ -4,11 +4,11 @@ def EXCLUDED_DIRS = ["bin", "build"]
// List all non-hidden directories not in EXCUDED_DIRS
ext.exampleFolderNames = file("${rootDir}")
- .listFiles()
- .findAll {
- return (it.isDirectory()
- && !it.isHidden()
- && !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
- && it.toPath().resolve("build.gradle").toFile().exists())
- }
- .collect { it.name }
+ .listFiles()
+ .findAll {
+ return (it.isDirectory()
+ && !it.isHidden()
+ && !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
+ && it.toPath().resolve("build.gradle").toFile().exists())
+ }
+ .collect { it.name }
diff --git a/photonlib-java-examples/getinrange/build.gradle b/photonlib-java-examples/getinrange/build.gradle
index 7d6e58895a..f3750f10b2 100644
--- a/photonlib-java-examples/getinrange/build.gradle
+++ b/photonlib-java-examples/getinrange/build.gradle
@@ -1,6 +1,6 @@
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
}
sourceCompatibility = JavaVersion.VERSION_11
@@ -78,7 +78,11 @@ wpi.sim.addDriverstation()
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
- from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
diff --git a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties
index 7f959b087a..24c4386e93 100644
--- a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties
+++ b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
-distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip
+distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
diff --git a/photonlib-java-examples/simaimandrange/2020-field.png b/photonlib-java-examples/simaimandrange/2020-field.png
new file mode 100644
index 0000000000..8b18648e82
Binary files /dev/null and b/photonlib-java-examples/simaimandrange/2020-field.png differ
diff --git a/photonlib-java-examples/simaimandrange/2020-infiniterecharge.json b/photonlib-java-examples/simaimandrange/2020-infiniterecharge.json
new file mode 100644
index 0000000000..7109f5c6e3
--- /dev/null
+++ b/photonlib-java-examples/simaimandrange/2020-infiniterecharge.json
@@ -0,0 +1,19 @@
+{
+ "game": "Infinite Recharge",
+ "field-image": "2020-field.png",
+ "field-corners": {
+ "top-left": [
+ 96,
+ 25
+ ],
+ "bottom-right": [
+ 1040,
+ 514
+ ]
+ },
+ "field-size": [
+ 52.4375,
+ 26.9375
+ ],
+ "field-unit": "foot"
+}
diff --git a/photonlib-java-examples/simaimandrange/build.gradle b/photonlib-java-examples/simaimandrange/build.gradle
index 7d6e58895a..f3750f10b2 100644
--- a/photonlib-java-examples/simaimandrange/build.gradle
+++ b/photonlib-java-examples/simaimandrange/build.gradle
@@ -1,6 +1,6 @@
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
}
sourceCompatibility = JavaVersion.VERSION_11
@@ -78,7 +78,11 @@ wpi.sim.addDriverstation()
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
- from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
diff --git a/photonlib-java-examples/simaimandrange/simgui.json b/photonlib-java-examples/simaimandrange/simgui.json
index 2ebe473318..4ff3cc0f5c 100644
--- a/photonlib-java-examples/simaimandrange/simgui.json
+++ b/photonlib-java-examples/simaimandrange/simgui.json
@@ -22,6 +22,7 @@
],
"weight": 3.0
},
+ "bottom": 514,
"cameras": {
"arrowColor": [
0.0,
@@ -33,7 +34,19 @@
"selectable": false,
"style": "Hidden"
},
+ "height": 8.210550308227539,
+ "image": "2020-field.png",
+ "left": 96,
+ "right": 1040,
"targets": {
+ "arrowColor": [
+ 0.27848100662231445,
+ 1.0,
+ 0.0,
+ 255.0
+ ],
+ "arrowSize": 58,
+ "arrowWeight": 6.0,
"color": [
0.05063295364379883,
1.0,
@@ -43,6 +56,8 @@
"style": "Hidden",
"weight": 1.0
},
+ "top": 25,
+ "width": 15.982950210571289,
"window": {
"visible": true
}
diff --git a/photonlib-java-examples/swervedriveposeestsim/2023-field.json b/photonlib-java-examples/swervedriveposeestsim/2023-field.json
new file mode 100644
index 0000000000..ff4c5ca238
--- /dev/null
+++ b/photonlib-java-examples/swervedriveposeestsim/2023-field.json
@@ -0,0 +1,19 @@
+{
+ "game": "Charged Up",
+ "field-image": "2023-field.png",
+ "field-corners": {
+ "top-left": [
+ 46,
+ 36
+ ],
+ "bottom-right": [
+ 1088,
+ 544
+ ]
+ },
+ "field-size": [
+ 54.27083,
+ 26.2916
+ ],
+ "field-unit": "foot"
+ }
diff --git a/photonlib-java-examples/swervedriveposeestsim/2023-field.png b/photonlib-java-examples/swervedriveposeestsim/2023-field.png
new file mode 100644
index 0000000000..ab3f0ff333
Binary files /dev/null and b/photonlib-java-examples/swervedriveposeestsim/2023-field.png differ
diff --git a/photonlib-java-examples/swervedriveposeestsim/build.gradle b/photonlib-java-examples/swervedriveposeestsim/build.gradle
index 424e9fa54a..a4d1b60579 100644
--- a/photonlib-java-examples/swervedriveposeestsim/build.gradle
+++ b/photonlib-java-examples/swervedriveposeestsim/build.gradle
@@ -1,6 +1,6 @@
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1"
+ id "edu.wpi.first.GradleRIO"
}
sourceCompatibility = JavaVersion.VERSION_11
@@ -78,7 +78,11 @@ wpi.sim.addDriverstation()
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
- from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+ from {
+ configurations.runtimeClasspath.collect {
+ it.isDirectory() ? it : zipTree(it)
+ }
+ }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
diff --git a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties
index d3fb630184..ca8f28abd3 100644
--- a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties
+++ b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
distributionBase=GRADLE_USER_HOME
distributionPath=permwrapper/dists
-distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip
+distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=permwrapper/dists
diff --git a/photonlib-java-examples/swervedriveposeestsim/simgui.json b/photonlib-java-examples/swervedriveposeestsim/simgui.json
index 23b949fb97..76fbf80d35 100644
--- a/photonlib-java-examples/swervedriveposeestsim/simgui.json
+++ b/photonlib-java-examples/swervedriveposeestsim/simgui.json
@@ -57,10 +57,12 @@
"weight": 2.0
},
"apriltag": {
+ "arrows": false,
"image": "tag-green.png",
- "length": 0.5,
- "width": 0.4000000059604645
+ "length": 0.6000000238418579,
+ "width": 0.5
},
+ "bottom": 544,
"cameras": {
"arrowColor": [
0.29535865783691406,
@@ -75,7 +77,11 @@
"weight": 1.0,
"width": 1.0
},
- "height": 8.013699531555176,
+ "height": 8.013679504394531,
+ "image": "2023-field.png",
+ "left": 46,
+ "right": 1088,
+ "top": 36,
"visibleTargetPoses": {
"arrows": false,
"image": "tag-blue.png",
@@ -83,7 +89,7 @@
"selectable": false,
"width": 0.4000000059604645
},
- "width": 16.541749954223633,
+ "width": 16.541748046875,
"window": {
"visible": true
}
diff --git a/shared/common.gradle b/shared/common.gradle
index e8181507e3..55f58760a9 100644
--- a/shared/common.gradle
+++ b/shared/common.gradle
@@ -1,31 +1,33 @@
-import java.nio.file.Path
-import java.time.LocalDateTime
-import java.time.format.DateTimeFormatter
-
// Plugins
-apply plugin: "jacoco"
apply plugin: "java"
+apply plugin: "jacoco"
java {
sourceCompatibility = JavaVersion.VERSION_11
targetCompatibility = JavaVersion.VERSION_11
}
-wpilibTools.deps.wpilibVersion = wpilibVersion
+wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
-dependencies {
- // Jackson
- implementation "com.fasterxml.jackson.core:jackson-annotations:2.12.4"
- implementation "com.fasterxml.jackson.core:jackson-core:2.12.4"
- implementation "com.fasterxml.jackson.core:jackson-databind:2.12.4"
+def nativeConfigName = 'wpilibTestNative'
+def nativeConfig = configurations.create(nativeConfigName)
- // Apache commons
- implementation group: "org.apache.commons", name: "commons-lang3", version: "3.12.0"
- implementation group: "commons-io", name: "commons-io", version: "2.11.0"
- implementation group: "commons-cli", name: "commons-cli", version: "1.5.0"
- implementation "org.apache.commons:commons-collections4:4.4"
- implementation "org.apache.commons:commons-exec:1.3"
+def nativeTasks = wpilibTools.createExtractionTasks {
+ configurationName = nativeConfigName
+}
+
+nativeTasks.addToSourceSetResources(sourceSets.main)
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+
+dependencies {
// WPILib deps
implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation wpilibTools.deps.wpilibJava("cameraserver")
@@ -35,14 +37,27 @@ dependencies {
implementation wpilibTools.deps.wpilibJava("ntcore")
implementation wpilibTools.deps.wpilibJava("hal")
implementation wpilibTools.deps.wpilibJava("wpilibj")
+ implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+
+ // Jackson
+ implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get()
+ implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get()
+ implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get()
- implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-java:$openCVversion"
- implementation "edu.wpi.first.thirdparty.frc2024.opencv:opencv-jni:$openCVversion:$jniPlatform"
+ // Apache commons
+ implementation group: "org.apache.commons", name: "commons-lang3", version: "3.12.0"
+ implementation group: "commons-io", name: "commons-io", version: "2.11.0"
+ implementation group: "commons-cli", name: "commons-cli", version: "1.5.0"
+ implementation "org.apache.commons:commons-collections4:4.4"
+ implementation "org.apache.commons:commons-exec:1.3"
- implementation "org.ejml:ejml-simple:0.42"
+ implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
+ implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get();
- // test stuff
testImplementation("org.junit.jupiter:junit-jupiter:5.8.2")
+
+ // wpilib serde
+ implementation 'us.hebi.quickbuf:quickbuf-runtime:1.3.2'
}
test {
@@ -54,7 +69,7 @@ test {
finalizedBy jacocoTestReport
}
-task testHeadless(type: Test) {
+tasks.register('testHeadless', Test) {
group = "verification"
systemProperty("java.awt.headless", "true")
useJUnitPlatform()
@@ -62,13 +77,12 @@ task testHeadless(type: Test) {
workingDir = "../"
}
-task generateJavaDocs(type: Javadoc) {
+tasks.register('generateJavaDocs', Javadoc) {
source = sourceSets.main.allJava
classpath = sourceSets.main.compileClasspath
destinationDir = file("${projectDir}/build/docs")
}
-
jacoco {
toolVersion = "0.8.9"
reportsDirectory = layout.buildDirectory.dir('customJacocoReportDir')
@@ -86,7 +100,7 @@ jacocoTestReport {
afterEvaluate {
classDirectories.setFrom(files(classDirectories.files.collect {
fileTree(dir: it,
- exclude: "edu/wpi/**"
+ exclude: "edu/wpi/**"
)
}))
}
diff --git a/shared/config.gradle b/shared/config.gradle
index 3d5f91b150..ca2df48acb 100644
--- a/shared/config.gradle
+++ b/shared/config.gradle
@@ -1,3 +1,5 @@
+apply plugin: "edu.wpi.first.NativeUtils"
+
// Configure Native-Utils WPI Plugin
nativeUtils.addWpiNativeUtils()
nativeUtils.withCrossRoboRIO()
@@ -6,15 +8,12 @@ nativeUtils.withCrossLinuxArm64()
// Configure WPI dependencies.
nativeUtils.wpi.configureDependencies {
+ wpiVersion = wpilibVersion
opencvYear = 'frc2024'
+ opencvVersion = openCVversion
googleTestYear = "frc2023"
-
- wpiVersion = wpilibVersion
- wpimathVersion = wpilibVersion
niLibVersion = "2024.1.1"
- opencvVersion = openCVversion
googleTestVersion = "1.12.1-2"
- imguiVersion = "1.86-1"
}
// Configure warnings and errors
@@ -78,7 +77,7 @@ ext.createComponentZipTasks = { components, names, base, type, project, func ->
if (it in NativeLibrarySpec && stringNames.contains(it.name)) {
it.binaries.each {
if (!it.buildable) return
- def target = nativeUtils.getPublishClassifier(it)
+ def target = nativeUtils.getPublishClassifier(it)
if (configMap.containsKey(target)) {
configMap.get(target).add(it)
} else {
@@ -128,7 +127,7 @@ ext.createAllCombined = { list, name, base, type, project ->
list.each {
if (it.name.endsWith('debug')) return
- from project.zipTree(it.archiveFile)
+ from project.zipTree(it.archiveFile)
dependsOn it
}
}
@@ -140,7 +139,6 @@ ext.createAllCombined = { list, name, base, type, project ->
}
return task
-
}
// Create the standard ZIP format for the dependencies.
diff --git a/shared/examples_common.gradle b/shared/examples_common.gradle
index b908c64b5e..5212307f0f 100644
--- a/shared/examples_common.gradle
+++ b/shared/examples_common.gradle
@@ -1,14 +1,6 @@
task copyPhotonlib() {
doFirst {
// Assume publish to maven local has just be run. Kinda curst
- // // first, invoke gradle to generate the vendor json
- // try {
- // exec {
- // commandLine 'gradlew', 'photon-lib:generateVendorJson', '-x', "check"
- // }
- // } catch (Exception e) {
- // // ignored
- // }
def vendorJsonSrc = new File("${rootDir}", "../photon-lib/build/generated/vendordeps/");
def vendorJsonDst = new File("${projectDir}/vendordeps/");
diff --git a/versioningHelper.gradle b/versioningHelper.gradle
index 4b085f3121..e9dd6e3d0a 100644
--- a/versioningHelper.gradle
+++ b/versioningHelper.gradle
@@ -3,7 +3,8 @@ import java.time.LocalDateTime
import java.time.format.DateTimeFormatter
gradle.allprojects {
- ext.getCurrentVersion = { ->
+ ext.getCurrentVersion = {
+ ->
def stdout = new ByteArrayOutputStream()
String tagIsh
try {