Skip to content

Commit

Permalink
[photon-lib] Cleanup simulation Rotation3d usage (#982)
Browse files Browse the repository at this point in the history
  • Loading branch information
@amquake
amquake committed Oct 26, 2023
1 parent 6314778 commit 5b2be11
Show file tree
Hide file tree
Showing 4 changed files with 29 additions and 154 deletions.
21 changes: 7 additions & 14 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,20 +181,13 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
new Transform3d(
new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
// match expected AprilTag coordinate system
// TODO cleanup coordinate systems in wpilib 2024
var apriltagTrl =
CoordinateSystem.convert(
camToTag.getTranslation(), CoordinateSystem.NWU(), CoordinateSystem.EDN());
var apriltagRot =
CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
.plus(
CoordinateSystem.convert(
camToTag.getRotation(),
CoordinateSystem.NWU(),
CoordinateSystem.EDN()));
apriltagRot = new Rotation3d(0, Math.PI, 0).plus(apriltagRot);
camToTag = new Transform3d(apriltagTrl, apriltagRot);
camToTag =
CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
// (AprilTag expects Z axis going into tag)
camToTag =
new Transform3d(
camToTag.getTranslation(),
new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation()));
tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
}
}
Expand Down
19 changes: 6 additions & 13 deletions photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.ArrayList;
Expand All @@ -49,6 +48,7 @@
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.CameraTargetRelation;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
Expand Down Expand Up @@ -202,23 +202,16 @@ public Mat getVideoSimFrameRaw() {
* @return If this vision target can be seen before image projection.
*/
public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) {
// var rel = new CameraTargetRelation(camPose, target.getPose());
// TODO: removal awaiting wpilib Rotation3d performance improvements
var relTarget = RotTrlTransform3d.makeRelativeTo(camPose).apply(target.getPose());
var camToTargYaw = new Rotation2d(relTarget.getX(), relTarget.getY());
var camToTargPitch =
new Rotation2d(Math.hypot(relTarget.getX(), relTarget.getY()), -relTarget.getZ());
var relCam = RotTrlTransform3d.makeRelativeTo(target.getPose()).apply(camPose);
var targToCamAngle = new Rotation2d(relCam.getX(), Math.hypot(relCam.getY(), relCam.getZ()));
var rel = new CameraTargetRelation(camPose, target.getPose());

return (
// target translation is outside of camera's FOV
(Math.abs(camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
&& (Math.abs(camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
(Math.abs(rel.camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
&& (Math.abs(rel.camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
&& (!target.getModel().isPlanar
|| Math.abs(targToCamAngle.getDegrees())
|| Math.abs(rel.targToCamAngle.getDegrees())
< 90) // camera is behind planar target and it should be occluded
&& (relTarget.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
&& (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
}

/**
Expand Down
27 changes: 9 additions & 18 deletions photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,8 @@
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 {
Expand All @@ -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;
}
Expand Down Expand Up @@ -219,39 +213,36 @@ public static <T> List<T> reorderCircular(List<T> elements, boolean backwards, i
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));
}

/**
* Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
* 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));
}

/**
* Convert a translation from EDN to NWU. For example, if you have a translation X,Y,Z {1, 0, 0}
* 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);
}

/**
* Convert a translation from NWU to EDN. For example, if you have a translation X,Y,Z {1, 0, 0}
* 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);
}

/**
Expand Down
116 changes: 7 additions & 109 deletions photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -31,11 +30,6 @@
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.
Expand All @@ -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));
}

/**
Expand All @@ -85,38 +66,6 @@ public 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.
Expand All @@ -129,12 +78,9 @@ public static RotTrlTransform3d makeRelativeTo(Pose3d pose) {

/** 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) */
Expand All @@ -153,74 +99,26 @@ public Rotation3d getRotation() {
}

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) {
return trls.stream().map(this::apply).collect(Collectors.toList());
}

public Rotation3d apply(Rotation3d rot) {
return rotate(rot);
return rot.plus(this.rot);
}

public List<Rotation3d> applyRots(List<Rotation3d> rots) {
return rots.stream().map(this::apply).collect(Collectors.toList());
}

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

0 comments on commit 5b2be11

Please sign in to comment.