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 java.util.List;
021import org.photonvision.vision.frame.Frame;
022import org.photonvision.vision.frame.FrameThresholdType;
023import org.photonvision.vision.opencv.Contour;
024import org.photonvision.vision.opencv.DualOffsetValues;
025import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
026import org.photonvision.vision.pipe.impl.*;
027import org.photonvision.vision.pipeline.result.CVPipelineResult;
028import org.photonvision.vision.target.PotentialTarget;
029import org.photonvision.vision.target.TargetOrientation;
030import org.photonvision.vision.target.TrackedTarget;
031
032/** Represents a pipeline for tracking retro-reflective targets. */
033public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectivePipelineSettings> {
034    private final FindContoursPipe findContoursPipe = new FindContoursPipe();
035    private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe();
036    private final FilterContoursPipe filterContoursPipe = new FilterContoursPipe();
037    private final GroupContoursPipe groupContoursPipe = new GroupContoursPipe();
038    private final SortContoursPipe sortContoursPipe = new SortContoursPipe();
039    private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
040    private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
041    private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
042    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
043
044    private final long[] pipeProfileNanos = new long[PipelineProfiler.ReflectivePipeCount];
045
046    private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.HSV;
047
048    public ReflectivePipeline() {
049        super(PROCESSING_TYPE);
050        settings = new ReflectivePipelineSettings();
051    }
052
053    public ReflectivePipeline(ReflectivePipelineSettings settings) {
054        super(PROCESSING_TYPE);
055        this.settings = settings;
056    }
057
058    @Override
059    protected void setPipeParamsImpl() {
060        var dualOffsetValues =
061                new DualOffsetValues(
062                        settings.offsetDualPointA,
063                        settings.offsetDualPointAArea,
064                        settings.offsetDualPointB,
065                        settings.offsetDualPointBArea);
066
067        var findContoursParams = new FindContoursPipe.FindContoursParams();
068        findContoursPipe.setParams(findContoursParams);
069
070        var speckleRejectParams =
071                new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
072        speckleRejectPipe.setParams(speckleRejectParams);
073
074        var filterContoursParams =
075                new FilterContoursPipe.FilterContoursParams(
076                        settings.contourArea,
077                        settings.contourRatio,
078                        settings.contourFullness,
079                        frameStaticProperties,
080                        settings.contourFilterRangeX,
081                        settings.contourFilterRangeY,
082                        settings.contourTargetOrientation == TargetOrientation.Landscape);
083        filterContoursPipe.setParams(filterContoursParams);
084
085        var groupContoursParams =
086                new GroupContoursPipe.GroupContoursParams(
087                        settings.contourGroupingMode, settings.contourIntersection);
088        groupContoursPipe.setParams(groupContoursParams);
089
090        var sortContoursParams =
091                new SortContoursPipe.SortContoursParams(
092                        settings.contourSortMode,
093                        settings.outputShowMultipleTargets ? MAX_MULTI_TARGET_RESULTS : 1,
094                        frameStaticProperties);
095        sortContoursPipe.setParams(sortContoursParams);
096
097        var collect2dTargetsParams =
098                new Collect2dTargetsPipe.Collect2dTargetsParams(
099                        settings.offsetRobotOffsetMode,
100                        settings.offsetSinglePoint,
101                        dualOffsetValues,
102                        settings.contourTargetOffsetPointEdge,
103                        settings.contourTargetOrientation,
104                        frameStaticProperties);
105        collect2dTargetsPipe.setParams(collect2dTargetsParams);
106
107        var cornerDetectionPipeParams =
108                new CornerDetectionPipe.CornerDetectionPipeParameters(
109                        settings.cornerDetectionStrategy,
110                        settings.cornerDetectionUseConvexHulls,
111                        settings.cornerDetectionExactSideCount,
112                        settings.cornerDetectionSideCount,
113                        settings.cornerDetectionAccuracyPercentage);
114        cornerDetectionPipe.setParams(cornerDetectionPipeParams);
115
116        var solvePNPParams =
117                new SolvePNPPipe.SolvePNPPipeParams(
118                        frameStaticProperties.cameraCalibration, settings.targetModel);
119        solvePNPPipe.setParams(solvePNPParams);
120    }
121
122    @Override
123    public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
124        long sumPipeNanosElapsed = 0L;
125
126        CVPipeResult<List<Contour>> findContoursResult =
127                findContoursPipe.run(frame.processedImage.getMat());
128        sumPipeNanosElapsed += pipeProfileNanos[2] = findContoursResult.nanosElapsed;
129
130        CVPipeResult<List<Contour>> speckleRejectResult =
131                speckleRejectPipe.run(findContoursResult.output);
132        sumPipeNanosElapsed += pipeProfileNanos[3] = speckleRejectResult.nanosElapsed;
133
134        CVPipeResult<List<Contour>> filterContoursResult =
135                filterContoursPipe.run(speckleRejectResult.output);
136        sumPipeNanosElapsed += pipeProfileNanos[4] = filterContoursResult.nanosElapsed;
137
138        CVPipeResult<List<PotentialTarget>> groupContoursResult =
139                groupContoursPipe.run(filterContoursResult.output);
140        sumPipeNanosElapsed += pipeProfileNanos[5] = groupContoursResult.nanosElapsed;
141
142        CVPipeResult<List<PotentialTarget>> sortContoursResult =
143                sortContoursPipe.run(groupContoursResult.output);
144        sumPipeNanosElapsed += pipeProfileNanos[6] = sortContoursResult.nanosElapsed;
145
146        CVPipeResult<List<TrackedTarget>> collect2dTargetsResult =
147                collect2dTargetsPipe.run(sortContoursResult.output);
148        sumPipeNanosElapsed += pipeProfileNanos[7] = collect2dTargetsResult.nanosElapsed;
149
150        List<TrackedTarget> targetList;
151
152        // 3d stuff
153        if (settings.solvePNPEnabled) {
154            var cornerDetectionResult = cornerDetectionPipe.run(collect2dTargetsResult.output);
155            sumPipeNanosElapsed += pipeProfileNanos[8] = cornerDetectionResult.nanosElapsed;
156
157            var solvePNPResult = solvePNPPipe.run(cornerDetectionResult.output);
158            sumPipeNanosElapsed += pipeProfileNanos[9] = solvePNPResult.nanosElapsed;
159
160            targetList = solvePNPResult.output;
161        } else {
162            pipeProfileNanos[8] = 0;
163            pipeProfileNanos[9] = 0;
164            targetList = collect2dTargetsResult.output;
165        }
166
167        var fpsResult = calculateFPSPipe.run(null);
168        var fps = fpsResult.output;
169
170        PipelineProfiler.printReflectiveProfile(pipeProfileNanos);
171
172        return new CVPipelineResult(frame.sequenceID, sumPipeNanosElapsed, fps, targetList, frame);
173    }
174}