Remove unused MathUtils.orthogonalizeRotationMatrix() (PhotonVision#944)

mdurrani808 · Oct 10, 2023 · 5f3b5d2 · 5f3b5d2
1 parent 0e06737
commit 5f3b5d2
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 62 deletions.
diff --git a/LICENSE_MathUtils_orthogonalizeRotationMatrix.txt b/LICENSE_MathUtils_orthogonalizeRotationMatrix.txt
diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
@@ -18,22 +18,17 @@
 package org.photonvision.common.util.math;
 
 import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.CoordinateSystem;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
 import java.util.List;
-import org.ejml.data.DMatrixRMaj;
-import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
-import org.ejml.simple.SimpleMatrix;
 import org.opencv.core.Mat;
 
 public class MathUtils {
@@ -232,38 +227,4 @@ public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
         var axis = rotation.getAxis().times(angle);
         rvecOutput.put(0, 0, axis.getData());
     }
-
-    /**
-     * Orthogonalize an input matrix using a QR decomposition. QR decompositions decompose a
-     * rectangular matrix 'A' such that 'A=QR', where Q is the closest orthogonal matrix to the input,
-     * and R is an upper triangular matrix.
-     *
-     * <p>The following function is released under the BSD license avaliable in
-     * LICENSE_MathUtils_orthogonalizeRotationMatrix.txt.
-     */
-    public static Matrix<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> input) {
-        var a = DecompositionFactory_DDRM.qr(3, 3);
-        if (!a.decompose(input.getStorage().getDDRM())) {
-            // best we can do is return the input
-            return input;
-        }
-
-        // Grab results (thanks for this _great_ api, EJML)
-        var Q = new DMatrixRMaj(3, 3);
-        var R = new DMatrixRMaj(3, 3);
-        a.getQ(Q, false);
-        a.getR(R, false);
-
-        // Fix signs in R if they're < 0 so it's close to an identity matrix
-        // (our QR decomposition implementation sometimes flips the signs of columns)
-        for (int colR = 0; colR < 3; ++colR) {
-            if (R.get(colR, colR) < 0) {
-                for (int rowQ = 0; rowQ < 3; ++rowQ) {
-                    Q.set(rowQ, colR, -Q.get(rowQ, colR));
-                }
-            }
-        }
-
-        return new Matrix<>(new SimpleMatrix(Q));
-    }
 }