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.target;
019
020import org.opencv.calib3d.Calib3d;
021import org.opencv.core.MatOfPoint2f;
022import org.opencv.core.Point;
023import org.opencv.core.RotatedRect;
024import org.opencv.core.TermCriteria;
025import org.photonvision.common.util.math.MathUtils;
026import org.photonvision.common.util.numbers.DoubleCouple;
027import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
028import org.photonvision.vision.opencv.DualOffsetValues;
029
030public class TargetCalculations {
031    /**
032     * Calculates the yaw and pitch of a point in the image. Yaw and pitch must be calculated together
033     * to account for perspective distortion. Yaw is positive right, and pitch is positive up.
034     *
035     * @param offsetCenterX The X value of the offset principal point (cx) in pixels
036     * @param targetCenterX The X value of the target's center point in pixels
037     * @param horizontalFocalLength The horizontal focal length (fx) in pixels
038     * @param offsetCenterY The Y value of the offset principal point (cy) in pixels
039     * @param targetCenterY The Y value of the target's center point in pixels
040     * @param verticalFocalLength The vertical focal length (fy) in pixels
041     * @param cameraCal Camera calibration parameters, or null if not calibrated
042     * @return The yaw and pitch from the principal axis to the target center, in degrees.
043     */
044    public static DoubleCouple calculateYawPitch(
045            double offsetCenterX,
046            double targetCenterX,
047            double horizontalFocalLength,
048            double offsetCenterY,
049            double targetCenterY,
050            double verticalFocalLength,
051            CameraCalibrationCoefficients cameraCal) {
052        if (cameraCal != null) {
053            // undistort
054            MatOfPoint2f temp = new MatOfPoint2f(new Point(targetCenterX, targetCenterY));
055            // Tighten up termination criteria
056            var termCriteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 1e-6);
057            Calib3d.undistortImagePoints(
058                    temp,
059                    temp,
060                    cameraCal.getCameraIntrinsicsMat(),
061                    cameraCal.getDistCoeffsMat(),
062                    termCriteria);
063            float buff[] = new float[2];
064            temp.get(0, 0, buff);
065            temp.release();
066
067            // if outside of the imager, convergence fails, or really really bad user camera cal,
068            // undistort will fail by giving us nans. at some point we should log this failure
069            // if we can't undistort, don't change the center location
070            if (Float.isFinite(buff[0]) && Float.isFinite(buff[1])) {
071                targetCenterX = buff[0];
072                targetCenterY = buff[1];
073            }
074        }
075
076        double yaw = Math.atan((targetCenterX - offsetCenterX) / horizontalFocalLength);
077        double pitch =
078                Math.atan((offsetCenterY - targetCenterY) / (verticalFocalLength / Math.cos(yaw)));
079        return new DoubleCouple(Math.toDegrees(yaw), Math.toDegrees(pitch));
080    }
081
082    public static double calculateSkew(boolean isLandscape, RotatedRect minAreaRect) {
083        // https://namkeenman.wordpress.com/2015/12/18/open-cv-determine-angle-of-rotatedrect-minarearect/
084        var angle = minAreaRect.angle;
085
086        if (isLandscape && minAreaRect.size.width < minAreaRect.size.height) angle += 90;
087        else if (!isLandscape && minAreaRect.size.height < minAreaRect.size.width) angle += 90;
088
089        // Ensure skew is bounded on [-90, 90]
090        while (angle > 90) angle -= 180;
091        while (angle < -90) angle += 180;
092
093        return angle;
094    }
095
096    public static Point calculateTargetOffsetPoint(
097            boolean isLandscape, TargetOffsetPointEdge offsetRegion, RotatedRect minAreaRect) {
098        Point[] vertices = new Point[4];
099
100        minAreaRect.points(vertices);
101
102        Point bottom = getMiddle(vertices[0], vertices[1]);
103        Point left = getMiddle(vertices[1], vertices[2]);
104        Point top = getMiddle(vertices[2], vertices[3]);
105        Point right = getMiddle(vertices[3], vertices[0]);
106
107        boolean orientationCorrect = minAreaRect.size.width > minAreaRect.size.height;
108        if (!isLandscape) orientationCorrect = !orientationCorrect;
109
110        switch (offsetRegion) {
111            case Top:
112                if (orientationCorrect) return (left.y < right.y) ? left : right;
113                else return (top.y < bottom.y) ? top : bottom;
114            case Bottom:
115                if (orientationCorrect) return (left.y > right.y) ? left : right;
116                else return (top.y > bottom.y) ? top : bottom;
117            case Left:
118                if (orientationCorrect) return (top.x < bottom.x) ? top : bottom;
119                else return (left.x < right.x) ? left : right;
120            case Right:
121                if (orientationCorrect) return (top.x > bottom.x) ? top : bottom;
122                else return (left.x > right.x) ? left : right;
123            default:
124                return minAreaRect.center;
125        }
126    }
127
128    private static Point getMiddle(Point p1, Point p2) {
129        return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2));
130    }
131
132    public static Point calculateRobotOffsetPoint(
133            Point offsetPoint,
134            Point camCenterPoint,
135            DualOffsetValues dualOffsetValues,
136            RobotOffsetPointMode offsetMode) {
137        switch (offsetMode) {
138            case None:
139            default:
140                return camCenterPoint;
141            case Single:
142                if (offsetPoint.x == 0 && offsetPoint.y == 0) {
143                    return camCenterPoint;
144                } else {
145                    return offsetPoint;
146                }
147            case Dual:
148                var resultPoint = new Point();
149                var lineValues = dualOffsetValues.getLineValues();
150                var offsetSlope = lineValues.getFirst();
151                var offsetIntercept = lineValues.getSecond();
152
153                resultPoint.x = (offsetPoint.x - offsetIntercept) / offsetSlope;
154                resultPoint.y = (offsetPoint.y * offsetSlope) + offsetIntercept;
155                return resultPoint;
156        }
157    }
158
159    public static double getAspectRatio(RotatedRect rect, boolean isLandscape) {
160        if (rect.size.width == 0 || rect.size.height == 0) return 0;
161        double ratio = rect.size.width / rect.size.height;
162
163        // In landscape, we should be shorter than we are wide (that is, aspect ratio should be >1)
164        if (isLandscape && ratio < 1) {
165            ratio = 1.0 / ratio;
166        }
167
168        // If portrait, should always be taller than wide (ratio < 1)
169        else if (!isLandscape && ratio > 1) {
170            ratio = 1.0 / ratio;
171        }
172
173        return ratio;
174    }
175
176    public static Point calculateDualOffsetCrosshair(
177            DualOffsetValues dualOffsetValues, double currentArea) {
178        boolean firstLarger = dualOffsetValues.firstPointArea >= dualOffsetValues.secondPointArea;
179        double upperArea =
180                firstLarger ? dualOffsetValues.secondPointArea : dualOffsetValues.firstPointArea;
181        double lowerArea =
182                firstLarger ? dualOffsetValues.firstPointArea : dualOffsetValues.secondPointArea;
183
184        var areaFraction = MathUtils.map(currentArea, lowerArea, upperArea, 0, 1);
185        var xLerp =
186                MathUtils.lerp(dualOffsetValues.firstPoint.x, dualOffsetValues.secondPoint.x, areaFraction);
187        var yLerp =
188                MathUtils.lerp(dualOffsetValues.firstPoint.y, dualOffsetValues.secondPoint.y, areaFraction);
189
190        return new Point(xLerp, yLerp);
191    }
192
193    public static DoubleCouple getLineFromPoints(Point firstPoint, Point secondPoint) {
194        var offsetLineSlope = (secondPoint.y - firstPoint.y) / (secondPoint.x - firstPoint.x);
195        var offsetLineIntercept = firstPoint.y - (offsetLineSlope * firstPoint.x);
196        return new DoubleCouple(offsetLineSlope, offsetLineIntercept);
197    }
198}