Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11: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

23
LICENSE_MathUtils_orthogonalizeRotationMatrix.txt
View File

@@ -1,23 +0,0 @@
Copyright (c) Photon Vision. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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));
}
}