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.aruco;
019
020import java.util.ArrayList;
021import java.util.Arrays;
022import java.util.Comparator;
023import org.opencv.core.Mat;
024import org.opencv.objdetect.ArucoDetector;
025import org.opencv.objdetect.DetectorParameters;
026import org.opencv.objdetect.Dictionary;
027import org.opencv.objdetect.Objdetect;
028import org.photonvision.common.logging.LogGroup;
029import org.photonvision.common.logging.Logger;
030import org.photonvision.vision.opencv.Releasable;
031
032/** This class wraps an {@link ArucoDetector} for convenience. */
033public class PhotonArucoDetector implements Releasable {
034    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
035
036    private static class ArucoDetectorHack extends ArucoDetector {
037        public ArucoDetectorHack(Dictionary predefinedDictionary) {
038            super(predefinedDictionary);
039        }
040
041        // avoid double-free by keeping track of this ourselves (ew)
042        private boolean freed = false;
043
044        @Override
045        public void finalize() throws Throwable {
046            if (freed) {
047                return;
048            }
049
050            super.finalize();
051            freed = true;
052        }
053    }
054
055    private final ArucoDetectorHack detector =
056            new ArucoDetectorHack(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5));
057
058    private final Mat ids = new Mat();
059    private final ArrayList<Mat> cornerMats = new ArrayList<>();
060
061    public ArucoDetector getDetector() {
062        return detector;
063    }
064
065    /**
066     * Get a copy of the current parameters being used. Must next call setParams to update the
067     * underlying detector object!
068     */
069    public DetectorParameters getParams() {
070        return detector.getDetectorParameters();
071    }
072
073    public void setParams(DetectorParameters params) {
074        detector.setDetectorParameters(params);
075    }
076
077    /**
078     * Detect fiducial tags in the grayscaled image using the {@link ArucoDetector} in this class.
079     * Parameters for detection can be modified with {@link #setParams(DetectorParameters)}.
080     *
081     * @param grayscaleImg A grayscaled image
082     * @return An array of ArucoDetectionResult, which contain tag corners and id.
083     */
084    public ArucoDetectionResult[] detect(Mat grayscaleImg) {
085        // detect tags
086        detector.detectMarkers(grayscaleImg, cornerMats, ids);
087
088        ArucoDetectionResult[] results = new ArucoDetectionResult[cornerMats.size()];
089        for (int i = 0; i < cornerMats.size(); i++) {
090            // each detection has a Mat of corners
091            Mat cornerMat = cornerMats.get(i);
092
093            // Aruco detection returns corners (BR, BL, TL, TR).
094            // For parity with AprilTags and photonlib, we want (BL, BR, TR, TL).
095            double[] xCorners = {
096                cornerMat.get(0, 1)[0],
097                cornerMat.get(0, 0)[0],
098                cornerMat.get(0, 3)[0],
099                cornerMat.get(0, 2)[0]
100            };
101            double[] yCorners = {
102                cornerMat.get(0, 1)[1],
103                cornerMat.get(0, 0)[1],
104                cornerMat.get(0, 3)[1],
105                cornerMat.get(0, 2)[1]
106            };
107            cornerMat.release();
108
109            results[i] = new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0]);
110        }
111
112        ids.release();
113
114        // sort tags by ID
115        Arrays.sort(results, Comparator.comparingInt(ArucoDetectionResult::getId));
116
117        return results;
118    }
119
120    @Override
121    public void release() {
122        try {
123            detector.finalize();
124        } catch (Throwable e) {
125            logger.error("Exception destroying PhotonArucoDetector", e);
126        }
127        ids.release();
128        for (var m : cornerMats) m.release();
129        cornerMats.clear();
130    }
131}