Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[photon-lib] Cleanup simulation Rotation3d usage #982

Merged
merged 4 commits into from
Oct 26, 2023
Merged

[photon-lib] Cleanup simulation Rotation3d usage #982

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
88675a3
cleanup RotTrlTransform3d usage
amquake Oct 25, 2023
cd01189
cleanup apriltag coordinate conversion
amquake Oct 25, 2023
8f704e0
format
amquake Oct 25, 2023
21456d5
Merge branch 'master' into sim-coordinates-2024
amquake Oct 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 7 additions & 14 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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);
}
}