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

[photon-lib] Cleanup simulation Rotation3d usage (#982)

Browse Source
This commit is contained in:
amquake
2023-10-25 18:19:48 -07:00
committed by GitHub
parent 63147786b9
commit 5b2be119e7
4 changed files with 29 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -49,8 +49,8 @@ import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
private static RotTrlTransform3d NWU_TO_EDN;
private static RotTrlTransform3d EDN_TO_NWU;
private static Rotation3d NWU_TO_EDN;
private static Rotation3d EDN_TO_NWU;
static {
try {
@@ -59,18 +59,12 @@ public final class OpenCVHelp {
throw new RuntimeException("Failed to load native libraries!", e);
}
NWU_TO_EDN =
new RotTrlTransform3d(
new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)),
new Translation3d());
EDN_TO_NWU =
new RotTrlTransform3d(
new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)),
new Translation3d());
NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0));
EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0));
}
public static Mat matrixToMat(SimpleMatrix matrix) {
var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
var mat = new Mat(matrix.getNumRows(), matrix.getNumCols(), CvType.CV_64F);
mat.put(0, 0, matrix.getDDRM().getData());
return mat;
}
@@ -219,14 +213,12 @@ public final class OpenCVHelp {
return reordered;
}
// TODO: RotTrlTransform3d removal awaiting Rotation3d performance improvements
/**
* Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
return new RotTrlTransform3d(EDN_TO_NWU.apply(rot), new Translation3d())
.apply(EDN_TO_NWU.inverse().getRotation());
return EDN_TO_NWU.unaryMinus().plus(rot.plus(EDN_TO_NWU));
}
/**
@@ -234,8 +226,7 @@ public final class OpenCVHelp {
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
return new RotTrlTransform3d(NWU_TO_EDN.apply(rot), new Translation3d())
.apply(NWU_TO_EDN.inverse().getRotation());
return NWU_TO_EDN.unaryMinus().plus(rot.plus(NWU_TO_EDN));
}
/**
@@ -243,7 +234,7 @@ public final class OpenCVHelp {
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Translation3d translationEDNtoNWU(Translation3d trl) {
return EDN_TO_NWU.apply(trl);
return trl.rotateBy(EDN_TO_NWU);
}
/**
@@ -251,7 +242,7 @@ public final class OpenCVHelp {
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Translation3d translationNWUtoEDN(Translation3d trl) {
return NWU_TO_EDN.apply(trl);
return trl.rotateBy(NWU_TO_EDN);
}
/**

116
photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
View File

@@ -18,7 +18,6 @@
package org.photonvision.estimation;
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.geometry.Translation3d;
@@ -31,11 +30,6 @@ import java.util.stream.Collectors;
public class RotTrlTransform3d {
private final Translation3d trl;
private final Rotation3d rot;
// TODO: removal awaiting wpilib Rotation3d performance improvements
private double m_w;
private double m_x;
private double m_y;
private double m_z;
/**
* A rotation-translation transformation.
@@ -48,25 +42,12 @@ public class RotTrlTransform3d {
*/
public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
this.rot = rot;
var quat = rot.getQuaternion();
m_w = quat.getW();
m_x = quat.getX();
m_y = quat.getY();
m_z = quat.getZ();
this.trl = trl;
}
public RotTrlTransform3d(Pose3d initial, Pose3d last) {
// this.rot = last.getRotation().minus(initial.getRotation());
// this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot));
var quat = initial.getRotation().getQuaternion();
m_w = quat.getW();
m_x = quat.getX();
m_y = quat.getY();
m_z = quat.getZ();
this.rot = invrotate(last.getRotation());
this.trl = last.getTranslation().minus(rotate(initial.getTranslation()));
this.rot = last.getRotation().minus(initial.getRotation());
this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot));
}
/**
@@ -85,38 +66,6 @@ public class RotTrlTransform3d {
this(new Rotation3d(), new Translation3d());
}
private Translation3d rotate(Translation3d otrl) {
final var p = new Quaternion(0.0, otrl.getX(), otrl.getY(), otrl.getZ());
final var qprime = times(times(p), new Quaternion(m_w, -m_x, -m_y, -m_z));
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
private Translation3d invrotate(Translation3d otrl) {
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
var result = rotate(otrl);
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
return result;
}
private Rotation3d rotate(Rotation3d orot) {
return new Rotation3d(times(orot.getQuaternion()));
}
private Rotation3d invrotate(Rotation3d orot) {
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
var result = rotate(orot);
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
return result;
}
/**
* The rotation-translation transformation that makes poses in the world consider this pose as the
* new origin, or change the basis to this pose.
@@ -129,12 +78,9 @@ public class RotTrlTransform3d {
/** The inverse of this transformation. Applying the inverse will "undo" this transformation. */
public RotTrlTransform3d inverse() {
// var inverseRot = rot.unaryMinus();
// var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
// return new RotTrlTransform3d(inverseRot, inverseTrl);
var inverseTrl = invrotate(trl).unaryMinus();
return new RotTrlTransform3d(new Rotation3d(new Quaternion(m_w, -m_x, -m_y, -m_z)), inverseTrl);
var inverseRot = rot.unaryMinus();
var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
return new RotTrlTransform3d(inverseRot, inverseTrl);
}
/** This transformation as a Transform3d (as if of the origin) */
@@ -153,8 +99,7 @@ public class RotTrlTransform3d {
}
public Translation3d apply(Translation3d trl) {
// return trl.rotateBy(rot).plus(this.trl);
return rotate(trl).plus(this.trl);
return trl.rotateBy(rot).plus(this.trl);
}
public List<Translation3d> applyTrls(List<Translation3d> trls) {
@@ -162,7 +107,7 @@ public class RotTrlTransform3d {
}
public Rotation3d apply(Rotation3d rot) {
return rotate(rot);
return rot.plus(this.rot);
}
public List<Rotation3d> applyRots(List<Rotation3d> rots) {
@@ -170,57 +115,10 @@ public class RotTrlTransform3d {
}
public Pose3d apply(Pose3d pose) {
// return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl),
// pose.getRotation().plus(rot));
return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation()));
}
public List<Pose3d> applyPoses(List<Pose3d> poses) {
return poses.stream().map(this::apply).collect(Collectors.toList());
}
// TODO: removal awaiting wpilib Rotation3d performance improvements
private Quaternion times(Quaternion other) {
final double o_w = other.getW();
final double o_x = other.getX();
final double o_y = other.getY();
final double o_z = other.getZ();
return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
}
private static Quaternion times(Quaternion a, Quaternion b) {
final double m_w = a.getW();
final double m_x = a.getX();
final double m_y = a.getY();
final double m_z = a.getZ();
final double o_w = b.getW();
final double o_x = b.getX();
final double o_y = b.getY();
final double o_z = b.getZ();
return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
}
private static Quaternion times(
double m_w,
double m_x,
double m_y,
double m_z,
double o_w,
double o_x,
double o_y,
double o_z) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
// v₁ x v₂
final double cross_x = m_y * o_z - o_y * m_z;
final double cross_y = o_x * m_z - m_x * o_z;
final double cross_z = m_x * o_y - o_x * m_y;
// v = w₁v₂ + w₂v₁ + v₁ x v₂
final double new_x = o_x * m_w + (m_x * o_w) + cross_x;
final double new_y = o_y * m_w + (m_y * o_w) + cross_y;
final double new_z = o_z * m_w + (m_z * o_w) + cross_z;
return new Quaternion(m_w * o_w - (m_x * o_x + m_y * o_y + m_z * o_z), new_x, new_y, new_z);
}
}