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

Initial AprilTag support (#458)

Browse Source 
(Very) beta AprilTag support in PhotonVision. Disables Picam GPU acceleration until we can debug auto exposure in the MMAL driver.

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
Co-authored-by: Chris <chrisgerth010592@gmail.com>
Co-authored-by: mdurrani808 <mdurrani808@gmail.com>
This commit is contained in:
shueja-personal
2022-09-28 18:21:41 -07:00
committed by GitHub
parent a3bcd3ac4f
commit a764ace7f2
114 changed files with 21488 additions and 10851 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
photon-targeting/src/main/java/edu/wpi/first/math/Vector.java Normal file
View File

@@ -0,0 +1,108 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math;
import edu.wpi.first.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
*
* <p>This class is intended to be used alongside the state space library.
*
* @param <R> The number of rows in this matrix.
*/
public class Vector<R extends Num> extends Matrix<R, N1> {
/**
* Constructs an empty zero vector of the given dimensions.
*
* @param rows The number of rows of the vector.
*/
public Vector(Nat<R> rows) {
super(rows, Nat.N1());
}
/**
* Constructs a new {@link Vector} with the given storage. Caller should make sure that the
* provided generic bounds match the shape of the provided {@link Vector}.
*
* <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
* absolutely necessary due to the desired function not being accessible through the {@link
* Vector} wrapper.
*
* @param storage The {@link SimpleMatrix} to back this vector.
*/
public Vector(SimpleMatrix storage) {
super(storage);
}
/**
* Constructs a new vector with the storage of the supplied matrix.
*
* @param other The {@link Vector} to copy the storage of.
*/
public Vector(Matrix<R, N1> other) {
super(other);
}
@Override
public Vector<R> times(double value) {
return new Vector<>(this.m_storage.scale(value));
}
@Override
public Vector<R> div(int value) {
return new Vector<>(this.m_storage.divide(value));
}
@Override
public Vector<R> div(double value) {
return new Vector<>(this.m_storage.divide(value));
}
/**
* Returns the dot product of this vector with another.
*
* @param other The other vector.
* @return The dot product.
*/
public double dot(Vector<R> other) {
double dot = 0.0;
for (int i = 0; i < getNumRows(); ++i) {
dot += get(i, 0) * other.get(i, 0);
}
return dot;
}
/**
* Returns the norm of this vector.
*
* @return The norm.
*/
public double norm() {
double squaredNorm = 0.0;
for (int i = 0; i < getNumRows(); ++i) {
squaredNorm += get(i, 0) * get(i, 0);
}
return Math.sqrt(squaredNorm);
}
}

99
photon-targeting/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java Normal file
View File

@@ -0,0 +1,99 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
/** A class representing a coordinate system axis within the NWU coordinate system. */
public class CoordinateAxis {
final Vector<N3> m_axis;
/**
* Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
*
* @param x The x component.
* @param y The y component.
* @param z The z component.
*/
public CoordinateAxis(double x, double y, double z) {
double norm = Math.sqrt(x * x + y * y + z * z);
m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
}
/**
* Returns a coordinate axis corresponding to +X in the NWU coordinate system.
*
* @return A coordinate axis corresponding to +X in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis N() {
return new CoordinateAxis(1.0, 0.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to -X in the NWU coordinate system.
*
* @return A coordinate axis corresponding to -X in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis S() {
return new CoordinateAxis(-1.0, 0.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
*
* @return A coordinate axis corresponding to -Y in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis E() {
return new CoordinateAxis(0.0, -1.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
*
* @return A coordinate axis corresponding to +Y in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis W() {
return new CoordinateAxis(0.0, 1.0, 0.0);
}
/**
* Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
*
* @return A coordinate axis corresponding to +Z in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis U() {
return new CoordinateAxis(0.0, 0.0, 1.0);
}
/**
* Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
*
* @return A coordinate axis corresponding to -Z in the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateAxis D() {
return new CoordinateAxis(0.0, 0.0, -1.0);
}
}

134
photon-targeting/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java Normal file
View File

@@ -0,0 +1,134 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
/** A helper class that converts Pose3d objects between different standard coordinate frames. */
public class CoordinateSystem {
// Rotation from this coordinate system to NWU coordinate system
private final Rotation3d m_rotation;
/**
* Constructs a coordinate system with the given cardinal directions for each axis.
*
* @param positiveX The cardinal direction of the positive x-axis.
* @param positiveY The cardinal direction of the positive y-axis.
* @param positiveZ The cardinal direction of the positive z-axis.
* @throws IllegalArgumentException if the coordinate system isn't special orthogonal
*/
public CoordinateSystem(
CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
// Construct a change of basis matrix from the source coordinate system to the
// NWU coordinate system. Each column vector in the change of basis matrix is
// one of the old basis vectors mapped to its representation in the new basis.
@SuppressWarnings("LocalVariableName")
var R = new Matrix<>(Nat.N3(), Nat.N3());
R.assignBlock(0, 0, positiveX.m_axis);
R.assignBlock(0, 1, positiveY.m_axis);
R.assignBlock(0, 2, positiveZ.m_axis);
// Require that the change of basis matrix is special orthogonal. This is true
// if the axes used are orthogonal and normalized. The Axis class already
// normalizes itself, so we just need to check for orthogonality.
if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
}
// Turn change of basis matrix into a quaternion since it's a pure rotation
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
double w;
double x;
double y;
double z;
if (trace > 0.0) {
double s = 0.5 / Math.sqrt(trace + 1.0);
w = 0.25 / s;
x = (R.get(2, 1) - R.get(1, 2)) * s;
y = (R.get(0, 2) - R.get(2, 0)) * s;
z = (R.get(1, 0) - R.get(0, 1)) * s;
} else {
if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
w = (R.get(2, 1) - R.get(1, 2)) / s;
x = 0.25 * s;
y = (R.get(0, 1) + R.get(1, 0)) / s;
z = (R.get(0, 2) + R.get(2, 0)) / s;
} else if (R.get(1, 1) > R.get(2, 2)) {
double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
w = (R.get(0, 2) - R.get(2, 0)) / s;
x = (R.get(0, 1) + R.get(1, 0)) / s;
y = 0.25 * s;
z = (R.get(1, 2) + R.get(2, 1)) / s;
} else {
double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
w = (R.get(1, 0) - R.get(0, 1)) / s;
x = (R.get(0, 2) + R.get(2, 0)) / s;
y = (R.get(1, 2) + R.get(2, 1)) / s;
z = 0.25 * s;
}
}
m_rotation = new Rotation3d(new Quaternion(w, x, y, z));
}
/**
* Returns an instance of the NWU coordinate system.
*
* @return An instance of the NWU coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateSystem NWU() {
return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
}
/**
* Returns an instance of the EDN coordinate system.
*
* @return An instance of the EDN coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateSystem EDN() {
return new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
}
/**
* Returns an instance of the NED coordinate system.
*
* @return An instance of the NED coordinate system.
*/
@SuppressWarnings("MethodName")
public static CoordinateSystem NED() {
return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
}
/**
* Converts the given pose from one coordinate system to another.
*
* @param pose The pose to convert.
* @param from The coordinate system the pose starts in.
* @param to The coordinate system to which to convert.
* @return The given pose in the desired coordinate system.
*/
public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
return pose.relativeTo(new Pose3d(new Translation3d(), to.m_rotation.minus(from.m_rotation)));
}
}

377
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java Normal file
View File

@@ -0,0 +1,377 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
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.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
public class Pose3d implements Interpolatable<Pose3d> {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs a pose with x, y, and z translations instead of a separate Translation3d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(double x, double y, double z, Rotation3d rotation) {
m_translation = new Translation3d(x, y, z);
m_rotation = rotation;
}
public Pose3d(Transform3d transform) {
this(transform.getTranslation(), transform.getRotation());
}
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d plus(Transform3d other) {
return transformBy(other);
}
/**
* Returns the Transform3d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform3d minus(Pose3d other) {
final var pose = this.relativeTo(other);
return new Transform3d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
public Translation3d getTranslation() {
return m_translation;
}
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
public double getX() {
return m_translation.getX();
}
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
public double getY() {
return m_translation.getY();
}
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose3d transformBy(Transform3d other) {
return new Pose3d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
* get the error between the reference and the current pose.
*
* @param other The pose that is the origin of the new coordinate frame that the current pose will
* be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose3d relativeTo(Pose3d other) {
var transform = new Transform3d(other, this);
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
* Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose3d exp(Twist3d twist) {
final var Omega = hatSO3(VecBuilder.fill(twist.droll, twist.dpitch, twist.dyaw));
final var OmegaSq = Omega.times(Omega);
double thetaSq =
twist.droll * twist.droll + twist.dpitch * twist.dpitch + twist.dyaw * twist.dyaw;
// Get left Jacobian of SO3. See first line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> J;
if (thetaSq < 1E-9 * 1E-9) {
// J = I + 0.5ω
J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
} else {
double theta = Math.sqrt(thetaSq);
// J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
J =
Matrix.eye(Nat.N3())
.plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
.plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
}
// Get translation component
final var translation =
J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
final var transform =
new Transform3d(
new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
new Rotation3d(twist.droll, twist.dpitch, twist.dyaw));
return this.plus(transform);
}
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output of a.Log(b), then
* a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
@SuppressWarnings("LocalVariableName")
public Twist3d log(Pose3d end) {
final var transform = end.relativeTo(this);
var rotVec = logSO3(transform.getRotation());
final var Omega = hatSO3(rotVec);
final var OmegaSq = Omega.times(Omega);
double thetaSq =
rotVec.get(0, 0) * rotVec.get(0, 0)
+ rotVec.get(1, 0) * rotVec.get(1, 0)
+ rotVec.get(2, 0) * rotVec.get(2, 0);
// Get left Jacobian inverse of SO3. See fourth line in right column of
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
Matrix<N3, N3> Jinv;
if (thetaSq < 1E-9 * 1E-9) {
// J^-1 = I − 0.5ω + 1/12 ω²
Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
} else {
double theta = Math.sqrt(thetaSq);
double halfTheta = 0.5 * theta;
// J^-1 = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
Jinv =
Matrix.eye(Nat.N3())
.minus(Omega.times(0.5))
.plus(
OmegaSq.times(
(1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
}
// Get dtranslation component
final var dtranslation =
Jinv.times(
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(transform.getX(), transform.getY(), transform.getZ()));
return new Twist3d(
dtranslation.get(0, 0),
dtranslation.get(1, 0),
dtranslation.get(2, 0),
rotVec.get(0, 0),
rotVec.get(1, 0),
rotVec.get(2, 0));
}
@Override
public String toString() {
return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
}
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*
* @return A Pose2d representing this Pose3d projected into the X-Y plane.
*/
public Pose2d toPose2d() {
return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
}
/**
* Checks equality between this Pose3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose3d) {
return ((Pose3d) obj).m_translation.equals(m_translation)
&& ((Pose3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
@Override
@SuppressWarnings("ParameterName")
public Pose3d interpolate(Pose3d endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
var twist = this.log(endValue);
var scaledTwist =
new Twist3d(
twist.dx * t,
twist.dy * t,
twist.dz * t,
twist.droll * t,
twist.dpitch * t,
twist.dyaw * t);
return this.exp(scaledTwist);
}
}
/**
* Applies the log operator to a rotation.
*
* <p>It takes a quaternion and returns a rotation vector.
*
* @param rotation The rotation.
* @return The rotation vector.
*/
private Vector<N3> logSO3(Rotation3d rotation) {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
final var q = rotation.getQuaternion();
double norm = Math.sqrt(q.getX() * q.getX() + q.getY() * q.getY() + q.getZ() * q.getZ());
// The quaternion in a Rotation3d is already guaranteed to have a nonzero
// vector component and be normalized, so no divide-by-zero checks are
// performed.
if (q.getW() < 0.0) {
return VecBuilder.fill(q.getX(), q.getY(), q.getZ())
.times(2.0 * Math.atan2(-norm, -q.getW()) / norm);
} else {
return VecBuilder.fill(q.getX(), q.getY(), q.getZ())
.times(2.0 * Math.atan2(norm, q.getW()) / norm);
}
}
/**
* Applies the hat operator to a rotation vector.
*
* <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
* algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
private Matrix<N3, N3> hatSO3(Vector<N3> rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return new MatBuilder<>(Nat.N3(), Nat.N3())
.fill(
0.0,
-rotation.get(2, 0),
rotation.get(1, 0),
rotation.get(2, 0),
0.0,
-rotation.get(0, 0),
-rotation.get(1, 0),
rotation.get(0, 0),
0.0);
}
}

178
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java Normal file
View File

@@ -0,0 +1,178 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.util.Arrays;
import java.util.Objects;
public class Quaternion {
private final double m_r;
private final Matrix<N3, N1> m_v;
/** Constructs a quaternion with a default angle of 0 degrees. */
public Quaternion() {
m_r = 1.0;
m_v = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0);
}
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
public Quaternion(double w, double x, double y, double z) {
m_r = w;
m_v = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(x, y, z);
}
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
* @return The quaternion product.
*/
public Quaternion times(Quaternion other) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
final var v1x = v1.get(0, 0);
final var v1y = v1.get(1, 0);
final var v1z = v1.get(2, 0);
final var v2x = v2.get(0, 0);
final var v2y = v2.get(1, 0);
final var v2z = v2.get(2, 0);
var cross =
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(v1y * v2z - v2y * v1z, v2x * v1z - v1x * v2z, v1x * v2y - v2x * v1y);
double dot = v1x * v2x + v1y * v2y + v1z * v2z;
final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
return new Quaternion(r1 * r2 - dot, v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
/**
* Checks equality between this Quaternion and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Quaternion) {
var other = (Quaternion) obj;
final var r1 = m_r;
final var v1 = m_v;
final var r2 = other.m_r;
final var v2 = other.m_v;
final var v1x = v1.get(0, 0);
final var v1y = v1.get(1, 0);
final var v1z = v1.get(2, 0);
final var v2x = v2.get(0, 0);
final var v2y = v2.get(1, 0);
final var v2z = v2.get(2, 0);
return Math.abs(r1 * r2 + v1x * v2x + v1y * v2y + v1z * v2z) > 1.0 - 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_r, m_v);
}
/**
* Returns the inverse of the quaternion.
*
* @return The inverse quaternion.
*/
public Quaternion inverse() {
return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
}
/**
* Normalizes the quaternion.
*
* @return The normalized quaternion.
*/
public Quaternion normalize() {
double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
if (norm == 0.0) {
return new Quaternion();
} else {
return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
}
}
/**
* Returns W component of the quaternion.
*
* @return W component of the quaternion.
*/
public double getW() {
return m_r;
}
/**
* Returns X component of the quaternion.
*
* @return X component of the quaternion.
*/
public double getX() {
return m_v.get(0, 0);
}
/**
* Returns Y component of the quaternion.
*
* @return Y component of the quaternion.
*/
public double getY() {
return m_v.get(1, 0);
}
/**
* Returns Z component of the quaternion.
*
* @return Z component of the quaternion.
*/
public double getZ() {
return m_v.get(2, 0);
}
@Override
public String toString() {
return "Quaternion{" + "m_r=" + m_r + ", m_v=" + Arrays.toString(m_v.getData()) + '}';
}
}

283
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java Normal file
View File

@@ -0,0 +1,283 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** A rotation in a 3D coordinate. */
public class Rotation3d implements Interpolatable<Rotation3d> {
private Quaternion m_q = new Quaternion();
/** Constructs a Rotation3d with a default angle of 0 degrees. */
public Rotation3d() {}
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
public Rotation3d(Quaternion q) {
m_q = q.normalize();
}
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
* than the body frame.
*
* @param roll The roll angle in radians.
* @param pitch The pitch angle in radians.
* @param yaw The yaw angle in radians.
*/
public Rotation3d(double roll, double pitch, double yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = Math.cos(roll * 0.5);
double sr = Math.sin(roll * 0.5);
double cp = Math.cos(pitch * 0.5);
double sp = Math.sin(pitch * 0.5);
double cy = Math.cos(yaw * 0.5);
double sy = Math.sin(yaw * 0.5);
m_q =
new Quaternion(
cr * cp * cy + sr * sp * sy,
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy);
}
/**
* Constructs a Rotation3d with the given axis and angle. The axis doesn't have to be normalized.
*
* @param axis The rotation axis.
* @param angleRadians The rotation around the axis in radians.
*/
public Rotation3d(Vector<N3> axis, double angleRadians) {
double norm = axis.normF();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
}
/**
* Constructs a quaternion from a 3x3, row-major direction cosine matrix
* https://intra.ece.ucr.edu/~farrell/AidedNavigation/D_App_Quaternions/Rot2Quat.pdf
*
* @param dcm A 3x3 direction cosine matrix
*/
public Rotation3d(Matrix<N3, N3> dcm) {
double b1 = 0.5 * Math.sqrt(1 + dcm.get(0, 0) + dcm.get(1, 1) + dcm.get(2, 2));
double b2 = (dcm.get(2, 1) - dcm.get(1, 2)) / (4 * b1);
double b3 = (dcm.get(0, 2) - dcm.get(2, 0)) / (4 * b1);
double b4 = (dcm.get(1, 0) - dcm.get(0, 1)) / (4 * b1);
m_q = new Quaternion(b1, b2, b3, b4).normalize();
}
/**
* Adds two rotations together.
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation3d plus(Rotation3d other) {
return rotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation3d minus(Rotation3d other) {
return rotateBy(other.unaryMinus());
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
public Rotation3d unaryMinus() {
return new Rotation3d(m_q.inverse());
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation3d.
*/
public Rotation3d times(double scalar) {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.getW() >= 0.0) {
return new Rotation3d(
VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
2.0 * scalar * Math.acos(m_q.getW()));
} else {
return new Rotation3d(
VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
2.0 * scalar * Math.acos(-m_q.getW()));
}
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
*
* @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
*/
public Rotation2d toRotation2d() {
return new Rotation2d(getZ());
}
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation3d.
*/
public Rotation3d rotateBy(Rotation3d other) {
return new Rotation3d(other.m_q.times(m_q));
}
/**
* Returns the quaternion representation of the Rotation3d.
*
* @return The quaternion representation of the Rotation3d.
*/
public Quaternion getQuaternion() {
return m_q;
}
/**
* Returns the counterclockwise rotation angle around the X axis (roll) in radians.
*
* @return The counterclockwise rotation angle around the X axis (roll) in radians.
*/
public double getX() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
}
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
*
* @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
*/
public double getY() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
double ratio = 2.0 * (w * y - z * x);
if (Math.abs(ratio) >= 1.0) {
return Math.copySign(Math.PI / 2.0, ratio);
} else {
return Math.asin(ratio);
}
}
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
*
* @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
*/
public double getZ() {
final var w = m_q.getW();
final var x = m_q.getX();
final var y = m_q.getY();
final var z = m_q.getZ();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}
/**
* Returns the axis in the axis-angle representation of this rotation.
*
* @return The axis in the axis-angle representation.
*/
public Vector<N3> getAxis() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
}
/**
* Returns the angle in radians in the axis-angle representation of this rotation.
*
* @return The angle in radians in the axis-angle representation of this rotation.
*/
public double getAngle() {
double norm =
Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
return 2.0 * Math.atan2(norm, m_q.getW());
}
/**
* Checks equality between this Rotation3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation3d) {
var other = (Rotation3d) obj;
return m_q.equals(other.m_q);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_q);
}
@Override
@SuppressWarnings("ParameterName")
public Rotation3d interpolate(Rotation3d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
@Override
public String toString() {
return String.format("Rotation3d(%s)", m_q);
}
}

165
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Transform3d.java Normal file
View File

@@ -0,0 +1,165 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import java.util.Objects;
/** Represents a transformation for a Pose3d. */
public class Transform3d {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform3d(Pose3d initial, Pose3d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
m_rotation = last.getRotation().minus(initial.getRotation());
}
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform3d(Translation3d translation, Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
public Transform3d times(double scalar) {
return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
public Transform3d plus(Transform3d other) {
return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation3d getTranslation() {
return m_translation;
}
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
public double getX() {
return m_translation.getX();
}
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
public double getY() {
return m_translation.getY();
}
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
public double getZ() {
return m_translation.getZ();
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation3d getRotation() {
return m_rotation;
}
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
public Transform3d inverse() {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return new Transform3d(
getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
getRotation().unaryMinus());
}
@Override
public String toString() {
return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Transform3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform3d) {
return ((Transform3d) obj).m_translation.equals(m_translation)
&& ((Transform3d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

235
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java Normal file
View File

@@ -0,0 +1,235 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/**
* Represents a translation in 3D space. This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes. When the robot is at the
* origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
* positive Z.
*/
@SuppressWarnings({"ParameterName", "MemberName"})
public class Translation3d implements Interpolatable<Translation3d> {
private final double m_x;
private final double m_y;
private final double m_z;
/** Constructs a Translation3d with X, Y, and Z components equal to zero. */
public Translation3d() {
this(0.0, 0.0, 0.0);
}
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
public Translation3d(double x, double y, double z) {
m_x = x;
m_y = y;
m_z = z;
}
/**
* Constructs a Translation3d with the provided distance and angle. This is essentially converting
* from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
public Translation3d(double distance, Rotation3d angle) {
final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
m_x = rectangular.getX();
m_y = rectangular.getY();
m_z = rectangular.getZ();
}
/**
* Calculates the distance between two translations in 3D space.
*
* <p>The distance between translations is defined as √((x2−x1)²+(y2−y1)²+(z2−z1)²).
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation3d other) {
return Math.sqrt(
Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
}
/**
* Returns the X component of the translation.
*
* @return The X component of the translation.
*/
public double getX() {
return m_x;
}
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
public double getY() {
return m_y;
}
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
public double getZ() {
return m_z;
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
/**
* Applies a rotation to the translation in 3D space.
*
* <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
* will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation3d rotateBy(Rotation3d other) {
final var p = new Quaternion(0.0, m_x, m_y, m_z);
final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
/**
* Returns the sum of two translations in 3D space.
*
* <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
* Translation3d{3.0, 8.0, 11.0).
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation3d plus(Translation3d other) {
return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
}
/**
* Returns the difference between two translations.
*
* <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
* Translation3d(4.0, 2.0, 0.0).
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation3d minus(Translation3d other) {
return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
}
/**
* Returns the inverse of the current translation. This is equivalent to negating all components
* of the translation.
*
* @return The inverse of the current translation.
*/
public Translation3d unaryMinus() {
return new Translation3d(-m_x, -m_y, -m_z);
}
/**
* Returns the translation multiplied by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation3d times(double scalar) {
return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
}
/**
* Returns the translation divided by a scalar.
*
* <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation3d div(double scalar) {
return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
}
@Override
public String toString() {
return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
}
/**
* Checks equality between this Translation3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation3d) {
return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9
&& Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y, m_z);
}
@Override
public Translation3d interpolate(Translation3d endValue, double t) {
return new Translation3d(
MathUtil.interpolate(this.getX(), endValue.getX(), t),
MathUtil.interpolate(this.getY(), endValue.getY(), t),
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*
* @return A Translation2d representing this Translation3d projected into the X-Y plane.
*/
public Translation2d toTranslation2d() {
return new Translation2d(m_x, m_y);
}
}

99
photon-targeting/src/main/java/edu/wpi/first/math/geometry/Twist3d.java Normal file
View File

@@ -0,0 +1,99 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.math.geometry;
import java.util.Objects;
/**
* A change in distance along a 3D arc since the last pose update. We can use ideas from
* differential calculus to create new Pose3ds from a Twist3d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist3d {
/** Linear "dx" component. */
public double dx;
/** Linear "dy" component. */
public double dy;
/** Linear "dz" component. */
public double dz;
/** Angular "droll" component (radians). */
public double droll;
/** Angular "dpitch" component (radians). */
public double dpitch;
/** Angular "dyaw" component (radians). */
public double dyaw;
public Twist3d() {}
/**
* Constructs a Twist3d with the given values.
*
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dz Change in z direction relative to robot.
* @param droll Change in roll relative to the robot.
* @param dpitch Change in pitch relative to the robot.
* @param dyaw Change in yaw relative to the robot.
*/
public Twist3d(double dx, double dy, double dz, double droll, double dpitch, double dyaw) {
this.dx = dx;
this.dy = dy;
this.dz = dz;
this.droll = droll;
this.dpitch = dpitch;
this.dyaw = dyaw;
}
@Override
public String toString() {
return String.format(
"Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, dRoll: %.2f, dPitch: %.2f, dYaw: %.2f)",
dx, dy, dz, droll, dpitch, dyaw);
}
/**
* Checks equality between this Twist3d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist3d) {
return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist3d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist3d) obj).dz - dz) < 1E-9
&& Math.abs(((Twist3d) obj).droll - droll) < 1E-9
&& Math.abs(((Twist3d) obj).dpitch - dpitch) < 1E-9
&& Math.abs(((Twist3d) obj).dyaw - dyaw) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dz, droll, dpitch, dyaw);
}
}

65
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -17,22 +17,24 @@
package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
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;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
public class PhotonTrackedTarget {
public static final int PACK_SIZE_BYTES = Double.BYTES * (7 + 2 * 4);
public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4);
private double yaw;
private double pitch;
private double area;
private double skew;
private Transform2d cameraToTarget = new Transform2d();
private int fiducialId;
private Transform3d cameraToTarget = new Transform3d();
private List<TargetCorner> targetCorners;
public PhotonTrackedTarget() {}
@@ -43,13 +45,15 @@ public class PhotonTrackedTarget {
double pitch,
double area,
double skew,
Transform2d pose,
int id,
Transform3d pose,
List<TargetCorner> corners) {
assert corners.size() == 4;
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
this.skew = skew;
this.fiducialId = id;
this.cameraToTarget = pose;
this.targetCorners = corners;
}
@@ -70,6 +74,11 @@ public class PhotonTrackedTarget {
return skew;
}
/** Get the Fiducial ID, or -1 if not set. */
public int getFiducialId() {
return fiducialId;
}
/**
* Return a list of the 4 corners in image space (origin top left, x left, y down), in no
* particular order, of the minimum area bounding rectangle of this target
@@ -78,7 +87,11 @@ public class PhotonTrackedTarget {
return targetCorners;
}
public Transform2d getCameraToTarget() {
/**
* Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag
* space (X right, Y up, Z towards the camera/out of the wall)
*/
public Transform3d getCameraToTarget() {
return cameraToTarget;
}
@@ -110,10 +123,17 @@ public class PhotonTrackedTarget {
this.pitch = packet.decodeDouble();
this.area = packet.decodeDouble();
this.skew = packet.decodeDouble();
this.fiducialId = packet.decodeInt();
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double r = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
this.targetCorners = new ArrayList<>(4);
for (int i = 0; i < 4; i++) {
@@ -122,7 +142,7 @@ public class PhotonTrackedTarget {
targetCorners.add(new TargetCorner(cx, cy));
}
this.cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
this.cameraToTarget = new Transform3d(translation, rotation);
return packet;
}
@@ -138,9 +158,14 @@ public class PhotonTrackedTarget {
packet.encode(pitch);
packet.encode(area);
packet.encode(skew);
packet.encode(fiducialId);
packet.encode(cameraToTarget.getTranslation().getX());
packet.encode(cameraToTarget.getTranslation().getY());
packet.encode(cameraToTarget.getRotation().getDegrees());
packet.encode(cameraToTarget.getTranslation().getZ());
packet.encode(cameraToTarget.getRotation().getQuaternion().getW());
packet.encode(cameraToTarget.getRotation().getQuaternion().getX());
packet.encode(cameraToTarget.getRotation().getQuaternion().getY());
packet.encode(cameraToTarget.getRotation().getQuaternion().getZ());
for (int i = 0; i < 4; i++) {
packet.encode(targetCorners.get(i).x);
@@ -149,4 +174,24 @@ public class PhotonTrackedTarget {
return packet;
}
@Override
public String toString() {
return "PhotonTrackedTarget{"
+ "yaw="
+ yaw
+ ", pitch="
+ pitch
+ ", area="
+ area
+ ", skew="
+ skew
+ ", fiducialId="
+ fiducialId
+ ", cameraToTarget="
+ cameraToTarget
+ ", targetCorners="
+ targetCorners
+ '}';
}
}