Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-26 01:51:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove unused MathUtils.orthogonalizeRotationMatrix() (#944)

Browse Source
This commit is contained in:
amquake
2023-10-10 04:12:03 -07:00
committed by GitHub
parent 0e06737272
commit 5f3b5d2f19
2 changed files with 0 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

@@ -18,7 +18,6 @@
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;
@@ -26,14 +25,10 @@ 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 class MathUtils {
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));
}
}