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.pipe.impl;
019
020import java.util.ArrayList;
021import java.util.Comparator;
022import java.util.List;
023import org.photonvision.vision.frame.FrameStaticProperties;
024import org.photonvision.vision.opencv.ContourSortMode;
025import org.photonvision.vision.pipe.CVPipe;
026import org.photonvision.vision.target.PotentialTarget;
027
028public class SortContoursPipe
029        extends CVPipe<
030                List<PotentialTarget>, List<PotentialTarget>, SortContoursPipe.SortContoursParams> {
031    private final List<PotentialTarget> m_sortedContours = new ArrayList<>();
032
033    @Override
034    protected List<PotentialTarget> process(List<PotentialTarget> in) {
035        for (var oldTarget : m_sortedContours) {
036            oldTarget.release();
037        }
038        m_sortedContours.clear();
039
040        if (!in.isEmpty()) {
041            m_sortedContours.addAll(in);
042            if (params.getSortMode() != ContourSortMode.Centermost) {
043                m_sortedContours.sort(params.getSortMode().getComparator());
044            } else {
045                // we need knowledge of camera properties to calculate this distance -- do it ourselves
046                m_sortedContours.sort(Comparator.comparingDouble(this::calcSquareCenterDistance));
047            }
048        }
049
050        return new ArrayList<>(
051                m_sortedContours.subList(0, Math.min(in.size(), params.getMaxTargets())));
052    }
053
054    private double calcSquareCenterDistance(PotentialTarget tgt) {
055        return Math.sqrt(
056                Math.pow(params.getCamProperties().centerX - tgt.getMinAreaRect().center.x, 2)
057                        + Math.pow(params.getCamProperties().centerY - tgt.getMinAreaRect().center.y, 2));
058    }
059
060    public static class SortContoursParams {
061        private final ContourSortMode m_sortMode;
062        private final int m_maxTargets;
063        private final FrameStaticProperties m_frameStaticProperties;
064
065        public SortContoursParams(
066                ContourSortMode sortMode, int maxTargets, FrameStaticProperties camProperties) {
067            m_sortMode = sortMode;
068            m_maxTargets = maxTargets;
069            m_frameStaticProperties = camProperties;
070        }
071
072        public ContourSortMode getSortMode() {
073            return m_sortMode;
074        }
075
076        public FrameStaticProperties getCamProperties() {
077            return m_frameStaticProperties;
078        }
079
080        public int getMaxTargets() {
081            return m_maxTargets;
082        }
083    }
084}