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.frame;
019
020import edu.wpi.first.cscore.VideoMode;
021import org.opencv.core.Point;
022import org.photonvision.common.util.numbers.DoubleCouple;
023import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
024import org.photonvision.vision.opencv.ImageRotationMode;
025
026/** Represents the properties of a frame. */
027public class FrameStaticProperties {
028    public final int imageWidth;
029    public final int imageHeight;
030    public final double fov;
031    public final double imageArea;
032    public final double centerX;
033    public final double centerY;
034    public final Point centerPoint;
035    public final double horizontalFocalLength;
036    public final double verticalFocalLength;
037    public CameraCalibrationCoefficients cameraCalibration;
038
039    // CameraCalibrationCoefficients hold native memory, so cache them here to avoid extra allocations
040    private final FrameStaticProperties[] cachedRotationStaticProperties =
041            new FrameStaticProperties[4];
042
043    /**
044     * Instantiates a new Frame static properties.
045     *
046     * @param mode The Video Mode of the camera.
047     * @param fov The FOV (Field Of Vision) of the image in degrees.
048     */
049    public FrameStaticProperties(VideoMode mode, double fov, CameraCalibrationCoefficients cal) {
050        this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cal);
051    }
052
053    /**
054     * Instantiates a new Frame static properties.
055     *
056     * @param imageWidth The width of the image in pixels.
057     * @param imageHeight The width of the image in pixels.
058     * @param fov The FOV (Field Of Vision) of the image in degrees.
059     */
060    public FrameStaticProperties(
061            int imageWidth, int imageHeight, double fov, CameraCalibrationCoefficients cal) {
062        this.imageWidth = imageWidth;
063        this.imageHeight = imageHeight;
064        this.fov = fov;
065        this.cameraCalibration = cal;
066
067        imageArea = this.imageWidth * this.imageHeight;
068
069        // pinhole model calculations
070        if (cameraCalibration != null && cameraCalibration.getCameraIntrinsicsMat() != null) {
071            // Use calibration data
072            var camIntrinsics = cameraCalibration.getCameraIntrinsicsMat();
073            centerX = camIntrinsics.get(0, 2)[0];
074            centerY = camIntrinsics.get(1, 2)[0];
075            centerPoint = new Point(centerX, centerY);
076            horizontalFocalLength = camIntrinsics.get(0, 0)[0];
077            verticalFocalLength = camIntrinsics.get(1, 1)[0];
078        } else {
079            // No calibration data. Calculate from user provided diagonal FOV
080            centerX = (this.imageWidth / 2.0) - 0.5;
081            centerY = (this.imageHeight / 2.0) - 0.5;
082            centerPoint = new Point(centerX, centerY);
083
084            DoubleCouple horizVertViews =
085                    calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight);
086            double horizFOV = Math.toRadians(horizVertViews.getFirst());
087            double vertFOV = Math.toRadians(horizVertViews.getSecond());
088            horizontalFocalLength = (this.imageWidth / 2.0) / Math.tan(horizFOV / 2.0);
089            verticalFocalLength = (this.imageHeight / 2.0) / Math.tan(vertFOV / 2.0);
090        }
091    }
092
093    public FrameStaticProperties rotate(ImageRotationMode rotation) {
094        if (rotation == ImageRotationMode.DEG_0) {
095            return this;
096        }
097
098        int newWidth = imageWidth;
099        int newHeight = imageHeight;
100
101        if (rotation == ImageRotationMode.DEG_90_CCW || rotation == ImageRotationMode.DEG_270_CCW) {
102            newWidth = imageHeight;
103            newHeight = imageWidth;
104        }
105
106        if (cameraCalibration == null) {
107            return new FrameStaticProperties(newWidth, newHeight, fov, null);
108        }
109
110        if (cachedRotationStaticProperties[rotation.ordinal()] == null) {
111            cachedRotationStaticProperties[rotation.ordinal()] =
112                    new FrameStaticProperties(
113                            newWidth, newHeight, fov, cameraCalibration.rotateCoefficients(rotation));
114        }
115
116        return cachedRotationStaticProperties[rotation.ordinal()];
117    }
118
119    /**
120     * Calculates the horizontal and vertical FOV components from a given diagonal FOV and image size.
121     *
122     * @param diagonalFoV Diagonal FOV in degrees
123     * @param imageWidth Image width in pixels
124     * @param imageHeight Image height in pixels
125     * @return Horizontal and vertical FOV in degrees
126     */
127    public static DoubleCouple calculateHorizontalVerticalFoV(
128            double diagonalFoV, int imageWidth, int imageHeight) {
129        diagonalFoV = Math.toRadians(diagonalFoV);
130        double diagonalAspect = Math.hypot(imageWidth, imageHeight);
131
132        double horizontalView =
133                Math.atan(Math.tan(diagonalFoV / 2) * (imageWidth / diagonalAspect)) * 2;
134        double verticalView = Math.atan(Math.tan(diagonalFoV / 2) * (imageHeight / diagonalAspect)) * 2;
135
136        return new DoubleCouple(Math.toDegrees(horizontalView), Math.toDegrees(verticalView));
137    }
138}