Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-06 03:31:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

[OpenCVHelp] Use correct Rotation3d constructor (#934)

Browse Source 
This fixes a occasional division by zero
This commit is contained in:
Matt
2023-10-09 09:44:45 -04:00
committed by GitHub
parent 2881741226
commit 6e8e3a0cba
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -27,7 +27,7 @@ package org.photonvision.estimation;
import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat; import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num; import edu.wpi.first.math.Num;
import edu.wpi.first.math.Vector; import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
@@ -142,16 +142,15 @@ public final class OpenCVHelp {
* @param rvecInput The rvec to create a Rotation3d from * @param rvecInput The rvec to create a Rotation3d from
*/ */
public static Rotation3d rvecToRotation(Mat rvecInput) { public static Rotation3d rvecToRotation(Mat rvecInput) {
// Get the 'rodriguez' (axis-angle, where the norm is the angle about the normalized direction
// of the vector)
float[] data = new float[3]; float[] data = new float[3];
var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F); var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F);
rvecInput.convertTo(wrapped, CvType.CV_32F); rvecInput.convertTo(wrapped, CvType.CV_32F);
wrapped.get(0, 0, data); wrapped.get(0, 0, data);
wrapped.release(); wrapped.release();
Vector<N3> axis = new Vector<>(Nat.N3());
axis.set(0, 0, data[0]); return rotationEDNtoNWU(new Rotation3d(VecBuilder.fill(data[0], data[1], data[2])));
axis.set(1, 0, data[1]);
axis.set(2, 0, data[2]);
return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm()));
} }
public static Point avgPoint(Point[] points) { public static Point avgPoint(Point[] points) {