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.calibration;
019
020import com.fasterxml.jackson.annotation.JsonIgnore;
021import com.fasterxml.jackson.annotation.JsonProperty;
022import edu.wpi.first.math.Matrix;
023import edu.wpi.first.math.Num;
024import java.util.Arrays;
025import org.ejml.simple.SimpleMatrix;
026import org.opencv.core.CvType;
027import org.opencv.core.Mat;
028import org.opencv.core.MatOfDouble;
029import org.photonvision.vision.opencv.Releasable;
030
031/** JSON-serializable image. Data is stored as a raw JSON array. */
032public class JsonMatOfDouble implements Releasable {
033    public final int rows;
034    public final int cols;
035    public final int type;
036    public final double[] data;
037
038    // Cached matrices to avoid object recreation
039    @JsonIgnore private Mat wrappedMat = null;
040    @JsonIgnore private Matrix wpilibMat = null;
041
042    @JsonIgnore private MatOfDouble wrappedMatOfDouble;
043    private boolean released = false;
044
045    public JsonMatOfDouble(int rows, int cols, double[] data) {
046        this(rows, cols, CvType.CV_64FC1, data);
047    }
048
049    public JsonMatOfDouble(
050            @JsonProperty("rows") int rows,
051            @JsonProperty("cols") int cols,
052            @JsonProperty("type") int type,
053            @JsonProperty("data") double[] data) {
054        this.rows = rows;
055        this.cols = cols;
056        this.type = type;
057        this.data = data;
058    }
059
060    @JsonIgnore
061    private static double[] getDataFromMat(Mat mat) {
062        double[] data = new double[(int) mat.total()];
063        mat.get(0, 0, data);
064        return data;
065    }
066
067    /**
068     * Returns a JsonMatOfDouble by copying the data from a Mat. The Mat type must be {@link
069     * CvType#CV_64FC1}.
070     *
071     * @param mat The Mat.
072     * @return The JsonMatOfDouble
073     */
074    public static JsonMatOfDouble fromMat(Mat mat) {
075        return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat));
076    }
077
078    @JsonIgnore
079    private Mat getAsMat() {
080        if (this.type != CvType.CV_64FC1) return null;
081
082        if (wrappedMat == null) {
083            this.wrappedMat = new Mat(this.rows, this.cols, this.type);
084            this.wrappedMat.put(0, 0, this.data);
085        }
086
087        if (this.released) {
088            throw new RuntimeException("This calibration object was already released");
089        }
090
091        return this.wrappedMat;
092    }
093
094    @JsonIgnore
095    public MatOfDouble getAsMatOfDouble() {
096        if (this.released) {
097            throw new RuntimeException("This calibration object was already released");
098        }
099
100        if (this.wrappedMatOfDouble == null) {
101            this.wrappedMatOfDouble = new MatOfDouble();
102            getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F);
103        }
104        return this.wrappedMatOfDouble;
105    }
106
107    @SuppressWarnings("unchecked")
108    @JsonIgnore
109    public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
110        if (wpilibMat == null) {
111            wpilibMat = new Matrix<R, C>(new SimpleMatrix(rows, cols, true, data));
112        }
113        return (Matrix<R, C>) wpilibMat;
114    }
115
116    @Override
117    public void release() {
118        if (wrappedMatOfDouble != null) {
119            wrappedMatOfDouble.release();
120        }
121
122        this.released = true;
123    }
124
125    @Override
126    public String toString() {
127        return "JsonMat [rows="
128                + rows
129                + ", cols="
130                + cols
131                + ", type="
132                + type
133                + ", data="
134                + Arrays.toString(data)
135                + ", wrappedMat="
136                + wrappedMat
137                + ", wpilibMat="
138                + wpilibMat
139                + ", wrappedMatOfDouble="
140                + wrappedMatOfDouble
141                + "]";
142    }
143}