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.apache.commons.lang3.tuple.Pair;
022import org.photonvision.vision.frame.Frame;
023import org.photonvision.vision.frame.FrameStaticProperties;
024import org.photonvision.vision.opencv.DualOffsetValues;
025import org.photonvision.vision.pipe.impl.*;
026import org.photonvision.vision.pipeline.result.CVPipelineResult;
027import org.photonvision.vision.target.TrackedTarget;
028
029/**
030 * This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It
031 * shall not get its settings saved, nor shall it be managed by PipelineManager
032 */
033public class OutputStreamPipeline {
034    private final OutputMatPipe outputMatPipe = new OutputMatPipe();
035    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
036    private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
037    private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
038    private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
039    private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
040    private final DrawCalibrationPipe drawCalibrationPipe = new DrawCalibrationPipe();
041
042    private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
043    private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
044    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
045    private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
046
047    private final long[] pipeProfileNanos = new long[12];
048
049    protected void setPipeParams(
050            FrameStaticProperties frameStaticProperties, AdvancedPipelineSettings settings) {
051        var dualOffsetValues =
052                new DualOffsetValues(
053                        settings.offsetDualPointA,
054                        settings.offsetDualPointAArea,
055                        settings.offsetDualPointB,
056                        settings.offsetDualPointBArea);
057
058        var draw2DTargetsParams =
059                new Draw2dTargetsPipe.Draw2dTargetsParams(
060                        settings.outputShouldDraw,
061                        settings.outputShowMultipleTargets,
062                        settings.streamingFrameDivisor);
063        draw2dTargetsPipe.setParams(draw2DTargetsParams);
064
065        var draw2DAprilTagsParams =
066                new Draw2dAprilTagsPipe.Draw2dAprilTagsParams(
067                        settings.outputShouldDraw,
068                        settings.outputShowMultipleTargets,
069                        settings.streamingFrameDivisor);
070        draw2dAprilTagsPipe.setParams(draw2DAprilTagsParams);
071
072        var draw2DArucoParams =
073                new Draw2dArucoPipe.Draw2dArucoParams(
074                        settings.outputShouldDraw,
075                        settings.outputShowMultipleTargets,
076                        settings.streamingFrameDivisor);
077        draw2dArucoPipe.setParams(draw2DArucoParams);
078
079        var draw2dCrosshairParams =
080                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
081                        settings.outputShouldDraw,
082                        settings.offsetRobotOffsetMode,
083                        settings.offsetSinglePoint,
084                        dualOffsetValues,
085                        frameStaticProperties,
086                        settings.streamingFrameDivisor,
087                        settings.inputImageRotationMode);
088        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
089
090        var draw3dTargetsParams =
091                new Draw3dTargetsPipe.Draw3dContoursParams(
092                        settings.outputShouldDraw,
093                        frameStaticProperties.cameraCalibration,
094                        settings.targetModel,
095                        settings.streamingFrameDivisor);
096        draw3dTargetsPipe.setParams(draw3dTargetsParams);
097
098        var draw3dAprilTagsParams =
099                new Draw3dAprilTagsPipe.Draw3dAprilTagsParams(
100                        settings.outputShouldDraw,
101                        frameStaticProperties.cameraCalibration,
102                        settings.targetModel,
103                        settings.streamingFrameDivisor);
104        draw3dAprilTagsPipe.setParams(draw3dAprilTagsParams);
105
106        var draw3dArucoParams =
107                new Draw3dArucoPipe.Draw3dArucoParams(
108                        settings.outputShouldDraw,
109                        frameStaticProperties.cameraCalibration,
110                        settings.targetModel,
111                        settings.streamingFrameDivisor);
112        draw3dArucoPipe.setParams(draw3dArucoParams);
113
114        resizeImagePipe.setParams(
115                new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
116
117        if (settings instanceof Calibration3dPipelineSettings) {
118            drawCalibrationPipe.setParams(
119                    new DrawCalibrationPipe.DrawCalibrationPipeParams(
120                            settings.streamingFrameDivisor,
121                            ((Calibration3dPipelineSettings) settings).drawAllSnapshots));
122        }
123    }
124
125    public CVPipelineResult process(
126            Frame inputAndOutputFrame,
127            AdvancedPipelineSettings settings,
128            List<TrackedTarget> targetsToDraw) {
129        setPipeParams(inputAndOutputFrame.frameStaticProperties, settings);
130        var inMat = inputAndOutputFrame.colorImage.getMat();
131        var outMat = inputAndOutputFrame.processedImage.getMat();
132
133        long sumPipeNanosElapsed = 0L;
134
135        // Resize both in place before doing any conversion
136        boolean inEmpty = inMat.empty();
137        if (!inEmpty)
138            sumPipeNanosElapsed += pipeProfileNanos[0] = resizeImagePipe.run(inMat).nanosElapsed;
139
140        boolean outEmpty = outMat.empty();
141        if (!outEmpty)
142            sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
143
144        // Only attempt drawing on a non-empty frame
145        if (!outEmpty) {
146            // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
147            if (outMat.channels() == 1) {
148                var outputMatPipeResult = outputMatPipe.run(outMat);
149                sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
150            } else {
151                pipeProfileNanos[2] = 0;
152            }
153
154            // Draw 2D Crosshair on output
155            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
156            sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
157
158            if (!(settings instanceof AprilTagPipelineSettings)
159                    && !(settings instanceof ArucoPipelineSettings)
160                    && !(settings instanceof Calibration3dPipelineSettings)) {
161                // If we're processing anything other than Apriltags..
162                var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
163                sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
164
165                if (settings.solvePNPEnabled) {
166                    // Draw 3D Targets on input and output if possible
167                    pipeProfileNanos[5] = 0;
168                    pipeProfileNanos[6] = 0;
169                    pipeProfileNanos[7] = 0;
170
171                    var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
172                    sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
173                } else {
174                    // Only draw 2d targets
175                    pipeProfileNanos[5] = 0;
176
177                    var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
178                    sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
179
180                    pipeProfileNanos[7] = 0;
181                    pipeProfileNanos[8] = 0;
182                }
183            } else if (settings instanceof Calibration3dPipelineSettings) {
184                pipeProfileNanos[5] = 0;
185                pipeProfileNanos[6] = 0;
186
187                var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw));
188                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
189
190                pipeProfileNanos[8] = 0;
191            } else if (settings instanceof AprilTagPipelineSettings) {
192                // If we are doing apriltags...
193                if (settings.solvePNPEnabled) {
194                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
195                    pipeProfileNanos[5] = 0;
196                    pipeProfileNanos[6] = 0;
197
198                    var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
199                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
200
201                    pipeProfileNanos[8] = 0;
202
203                } else {
204                    // Draw 2d apriltag markers
205                    var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
206                    sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
207
208                    pipeProfileNanos[6] = 0;
209                    pipeProfileNanos[7] = 0;
210                    pipeProfileNanos[8] = 0;
211                }
212            } else if (settings instanceof ArucoPipelineSettings) {
213                if (settings.solvePNPEnabled) {
214                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
215                    pipeProfileNanos[5] = 0;
216                    pipeProfileNanos[6] = 0;
217
218                    var drawOnInputResult = draw3dArucoPipe.run(Pair.of(outMat, targetsToDraw));
219                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
220
221                    pipeProfileNanos[8] = 0;
222
223                } else {
224                    // Draw 2d apriltag markers
225                    var draw2dTargetsOnInput = draw2dArucoPipe.run(Pair.of(outMat, targetsToDraw));
226                    sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
227
228                    pipeProfileNanos[6] = 0;
229                    pipeProfileNanos[7] = 0;
230                    pipeProfileNanos[8] = 0;
231                }
232            }
233        }
234
235        var fpsResult = calculateFPSPipe.run(null);
236        var fps = fpsResult.output;
237
238        return new CVPipelineResult(
239                inputAndOutputFrame.sequenceID,
240                sumPipeNanosElapsed,
241                fps, // Unused but here just in case
242                targetsToDraw,
243                inputAndOutputFrame);
244    }
245}