001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.vision.pipeline;
019
020import edu.wpi.first.apriltag.AprilTagDetection;
021import edu.wpi.first.apriltag.AprilTagDetector;
022import edu.wpi.first.apriltag.AprilTagPoseEstimate;
023import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
024import edu.wpi.first.math.geometry.CoordinateSystem;
025import edu.wpi.first.math.geometry.Pose3d;
026import edu.wpi.first.math.geometry.Rotation3d;
027import edu.wpi.first.math.geometry.Transform3d;
028import edu.wpi.first.math.util.Units;
029import java.util.ArrayList;
030import java.util.List;
031import java.util.Optional;
032import org.photonvision.common.configuration.ConfigManager;
033import org.photonvision.common.util.math.MathUtils;
034import org.photonvision.estimation.TargetModel;
035import org.photonvision.targeting.MultiTargetPNPResult;
036import org.photonvision.vision.apriltag.AprilTagFamily;
037import org.photonvision.vision.frame.Frame;
038import org.photonvision.vision.frame.FrameThresholdType;
039import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
040import org.photonvision.vision.pipe.impl.AprilTagDetectionPipe;
041import org.photonvision.vision.pipe.impl.AprilTagDetectionPipeParams;
042import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe;
043import org.photonvision.vision.pipe.impl.AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams;
044import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
045import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe;
046import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipeParams;
047import org.photonvision.vision.pipeline.result.CVPipelineResult;
048import org.photonvision.vision.target.TrackedTarget;
049import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
050
051public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipelineSettings> {
052    private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe();
053    private final AprilTagPoseEstimatorPipe singleTagPoseEstimatorPipe =
054            new AprilTagPoseEstimatorPipe();
055    private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe();
056    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
057
058    private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
059
060    public AprilTagPipeline() {
061        super(PROCESSING_TYPE);
062        settings = new AprilTagPipelineSettings();
063    }
064
065    public AprilTagPipeline(AprilTagPipelineSettings settings) {
066        super(PROCESSING_TYPE);
067        this.settings = settings;
068    }
069
070    @Override
071    protected void setPipeParamsImpl() {
072        // Sanitize thread count - not supported to have fewer than 1 threads
073        settings.threads = Math.max(1, settings.threads);
074
075        // for now, hard code tag width based on enum value
076        // 2023/other: best guess is 6in
077        double tagWidth = Units.inchesToMeters(6);
078        TargetModel tagModel = TargetModel.kAprilTag16h5;
079        if (settings.tagFamily == AprilTagFamily.kTag36h11) {
080            // 2024 tag, 6.5in
081            tagWidth = Units.inchesToMeters(6.5);
082            tagModel = TargetModel.kAprilTag36h11;
083        }
084
085        var config = new AprilTagDetector.Config();
086        config.numThreads = settings.threads;
087        config.refineEdges = settings.refineEdges;
088        config.quadSigma = (float) settings.blur;
089        config.quadDecimate = settings.decimate;
090
091        var quadParams = new AprilTagDetector.QuadThresholdParameters();
092        // 5 was the default minClusterPixels in WPILib prior to 2025
093        // increasing it causes detection problems when decimate > 1
094        quadParams.minClusterPixels = 5;
095        // these are the same as the values in WPILib 2025
096        // setting them here to prevent upstream changes from changing behavior of the detector
097        quadParams.maxNumMaxima = 10;
098        quadParams.criticalAngle = 45 * Math.PI / 180.0;
099        quadParams.maxLineFitMSE = 10.0f;
100        quadParams.minWhiteBlackDiff = 5;
101        quadParams.deglitch = false;
102
103        aprilTagDetectionPipe.setParams(
104                new AprilTagDetectionPipeParams(settings.tagFamily, config, quadParams));
105
106        if (frameStaticProperties.cameraCalibration != null) {
107            var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
108            if (cameraMatrix != null && cameraMatrix.rows() > 0) {
109                var cx = cameraMatrix.get(0, 2)[0];
110                var cy = cameraMatrix.get(1, 2)[0];
111                var fx = cameraMatrix.get(0, 0)[0];
112                var fy = cameraMatrix.get(1, 1)[0];
113
114                singleTagPoseEstimatorPipe.setParams(
115                        new AprilTagPoseEstimatorPipeParams(
116                                new Config(tagWidth, fx, fy, cx, cy),
117                                frameStaticProperties.cameraCalibration,
118                                settings.numIterations));
119
120                // TODO global state ew
121                var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
122                multiTagPNPPipe.setParams(
123                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
124            }
125        }
126    }
127
128    @Override
129    protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings settings) {
130        long sumPipeNanosElapsed = 0L;
131
132        if (frame.type != FrameThresholdType.GREYSCALE) {
133            // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up
134            return new CVPipelineResult(frame.sequenceID, 0, 0, List.of(), frame);
135        }
136
137        CVPipeResult<List<AprilTagDetection>> tagDetectionPipeResult;
138        tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage);
139        sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed;
140
141        List<AprilTagDetection> detections = tagDetectionPipeResult.output;
142        List<AprilTagDetection> usedDetections = new ArrayList<>();
143        List<TrackedTarget> targetList = new ArrayList<>();
144
145        // Filter out detections based on pipeline settings
146        for (AprilTagDetection detection : detections) {
147            // TODO this should be in a pipe, not in the top level here (Matt)
148            if (detection.getDecisionMargin() < settings.decisionMargin) continue;
149            if (detection.getHamming() > settings.hammingDist) continue;
150
151            usedDetections.add(detection);
152
153            // Populate target list for multitag
154            // (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should
155            // not be necessary.)
156            TrackedTarget target =
157                    new TrackedTarget(
158                            detection,
159                            null,
160                            new TargetCalculationParameters(
161                                    false, null, null, null, null, frameStaticProperties));
162
163            targetList.add(target);
164        }
165
166        // Do multi-tag pose estimation
167        Optional<MultiTargetPNPResult> multiTagResult = Optional.empty();
168        if (settings.solvePNPEnabled && settings.doMultiTarget) {
169            var multiTagOutput = multiTagPNPPipe.run(targetList);
170            sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
171            multiTagResult = multiTagOutput.output;
172        }
173
174        // Do single-tag pose estimation
175        if (settings.solvePNPEnabled) {
176            // Clear target list that was used for multitag so we can add target transforms
177            targetList.clear();
178            // TODO global state again ew
179            var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
180
181            for (AprilTagDetection detection : usedDetections) {
182                AprilTagPoseEstimate tagPoseEstimate = null;
183                // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
184                if (settings.doSingleTargetAlways
185                        || !(multiTagResult.isPresent()
186                                && multiTagResult.get().fiducialIDsUsed.contains((short) detection.getId()))) {
187                    var poseResult = singleTagPoseEstimatorPipe.run(detection);
188                    sumPipeNanosElapsed += poseResult.nanosElapsed;
189                    tagPoseEstimate = poseResult.output;
190                }
191
192                // If single-tag estimation was not done, this is a multi-target tag from the layout
193                if (tagPoseEstimate == null && multiTagResult.isPresent()) {
194                    // compute this tag's camera-to-tag transform using the multitag result
195                    var tagPose = atfl.getTagPose(detection.getId());
196                    if (tagPose.isPresent()) {
197                        var camToTag =
198                                new Transform3d(
199                                        new Pose3d().plus(multiTagResult.get().estimatedPose.best), tagPose.get());
200                        // match expected AprilTag coordinate system
201                        camToTag =
202                                CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
203                        // (AprilTag expects Z axis going into tag)
204                        camToTag =
205                                new Transform3d(
206                                        camToTag.getTranslation(),
207                                        new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation()));
208                        tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
209                    }
210                }
211
212                // populate the target list
213                // Challenge here is that TrackedTarget functions with OpenCV Contour
214                TrackedTarget target =
215                        new TrackedTarget(
216                                detection,
217                                tagPoseEstimate,
218                                new TargetCalculationParameters(
219                                        false, null, null, null, null, frameStaticProperties));
220
221                var correctedBestPose =
222                        MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
223                var correctedAltPose =
224                        MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());
225
226                target.setBestCameraToTarget3d(
227                        new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
228                target.setAltCameraToTarget3d(
229                        new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation()));
230
231                targetList.add(target);
232            }
233        }
234
235        var fpsResult = calculateFPSPipe.run(null);
236        var fps = fpsResult.output;
237
238        return new CVPipelineResult(
239                frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
240    }
241
242    @Override
243    public void release() {
244        aprilTagDetectionPipe.release();
245        singleTagPoseEstimatorPipe.release();
246        super.release();
247    }
248}