From 916431b4fff5b4fa7b2ea5d02d4327bfcf1b4201 Mon Sep 17 00:00:00 2001 From: shueja-personal <32416547+shueja-personal@users.noreply.github.com> Date: Wed, 9 Nov 2022 11:00:46 -0800 Subject: [PATCH] JSONify the bundled 3D geometry (#578) Co-authored-by: Matt M --- photon-targeting/build.gradle | 2 + .../edu/wpi/first/math/geometry/Pose3d.java | 14 ++- .../wpi/first/math/geometry/Quaternion.java | 97 +++++++++++-------- .../wpi/first/math/geometry/Rotation3d.java | 12 ++- .../first/math/geometry/Translation3d.java | 16 ++- 5 files changed, 97 insertions(+), 44 deletions(-) diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 86bd05006..6cb84a95d 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -1,5 +1,7 @@ apply plugin: "java" +apply from: "${rootDir}/shared/common.gradle" + sourceCompatibility = 11 targetCompatibility = 11 diff --git a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 1f18b7eb1..50b76d9dd 100644 --- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -17,6 +17,10 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -27,6 +31,8 @@ import edu.wpi.first.math.numbers.N3; import java.util.Objects; /** Represents a 3D pose containing translational and rotational elements. */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Pose3d implements Interpolatable { private final Translation3d m_translation; private final Rotation3d m_rotation; @@ -43,11 +49,13 @@ public class Pose3d implements Interpolatable { * @param translation The translational component of the pose. * @param rotation The rotational component of the pose. */ - public Pose3d(Translation3d translation, Rotation3d rotation) { + @JsonCreator + public Pose3d( + @JsonProperty(required = true, value = "translation") Translation3d translation, + @JsonProperty(required = true, value = "rotation") Rotation3d rotation) { m_translation = translation; m_rotation = rotation; } - /** * Constructs a pose with x, y, and z translations instead of a separate Translation3d. * @@ -91,6 +99,7 @@ public class Pose3d implements Interpolatable { * * @return The translational component of the pose. */ + @JsonProperty public Translation3d getTranslation() { return m_translation; } @@ -127,6 +136,7 @@ public class Pose3d implements Interpolatable { * * @return The rotational component of the pose. */ + @JsonProperty public Rotation3d getRotation() { return m_rotation; } diff --git a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 038694562..eb3fb2543 100644 --- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -17,22 +17,25 @@ 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 com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.numbers.N3; -import java.util.Arrays; import java.util.Objects; +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Quaternion { private final double m_r; - private final Matrix m_v; + private final Vector 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); + m_v = VecBuilder.fill(0.0, 0.0, 0.0); } /** @@ -43,9 +46,14 @@ public class 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) { + @JsonCreator + public Quaternion( + @JsonProperty(required = true, value = "W") double w, + @JsonProperty(required = true, value = "X") double x, + @JsonProperty(required = true, value = "Y") double y, + @JsonProperty(required = true, value = "Z") double z) { m_r = w; - m_v = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(x, y, z); + m_v = VecBuilder.fill(x, y, z); } /** @@ -61,21 +69,23 @@ public class Quaternion { 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); - + // v₁ x v₂ 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; + VecBuilder.fill( + v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0), + v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0), + v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0)); + // v = r₁v₂ + r₂v₁ + v₁ x v₂ 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)); + + return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0)); + } + + @Override + public String toString() { + return String.format( + "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0)); } /** @@ -89,20 +99,7 @@ public class Quaternion { 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 Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9; } return false; } @@ -140,6 +137,7 @@ public class Quaternion { * * @return W component of the quaternion. */ + @JsonProperty(value = "W") public double getW() { return m_r; } @@ -149,6 +147,7 @@ public class Quaternion { * * @return X component of the quaternion. */ + @JsonProperty(value = "X") public double getX() { return m_v.get(0, 0); } @@ -158,6 +157,7 @@ public class Quaternion { * * @return Y component of the quaternion. */ + @JsonProperty(value = "Y") public double getY() { return m_v.get(1, 0); } @@ -167,12 +167,33 @@ public class Quaternion { * * @return Z component of the quaternion. */ + @JsonProperty(value = "Z") 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()) + '}'; + /** + * Returns the rotation vector representation of this quaternion. + * + *

This is also the log operator of SO(3). + * + * @return The rotation vector representation of this quaternion. + */ + public Vector toRotationVector() { + // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with + // Sound State Representation through Encapsulation of Manifolds" + // + // https://arxiv.org/pdf/1107.1119.pdf + double norm = m_v.norm(); + + if (norm < 1e-9) { + return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW())); + } else { + if (getW() < 0.0) { + return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm); + } else { + return m_v.times(2.0 * Math.atan2(norm, getW()) / norm); + } + } } } diff --git a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index e0b9f8866..5f02bf4bc 100644 --- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -17,6 +17,10 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -25,7 +29,9 @@ import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; import java.util.Objects; -/** A rotation in a 3D coordinate. */ +/** A rotation in a 3D coordinate frame represented by a quaternion. */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation3d implements Interpolatable { private Quaternion m_q = new Quaternion(); @@ -37,7 +43,8 @@ public class Rotation3d implements Interpolatable { * * @param q The quaternion. */ - public Rotation3d(Quaternion q) { + @JsonCreator + public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) { m_q = q.normalize(); } @@ -174,6 +181,7 @@ public class Rotation3d implements Interpolatable { * * @return The quaternion representation of the Rotation3d. */ + @JsonProperty(value = "quaternion") public Quaternion getQuaternion() { return m_q; } diff --git a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 19b396cf4..e96dd1200 100644 --- a/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/photon-targeting/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -17,6 +17,10 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; @@ -28,7 +32,8 @@ import java.util.Objects; * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is * positive Z. */ -@SuppressWarnings({"ParameterName", "MemberName"}) +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation3d implements Interpolatable { private final double m_x; private final double m_y; @@ -46,7 +51,11 @@ public class Translation3d implements Interpolatable { * @param y The y component of the translation. * @param z The z component of the translation. */ - public Translation3d(double x, double y, double z) { + @JsonCreator + public Translation3d( + @JsonProperty(required = true, value = "x") double x, + @JsonProperty(required = true, value = "y") double y, + @JsonProperty(required = true, value = "z") double z) { m_x = x; m_y = y; m_z = z; @@ -84,6 +93,7 @@ public class Translation3d implements Interpolatable { * * @return The X component of the translation. */ + @JsonProperty public double getX() { return m_x; } @@ -93,6 +103,7 @@ public class Translation3d implements Interpolatable { * * @return The Y component of the translation. */ + @JsonProperty public double getY() { return m_y; } @@ -102,6 +113,7 @@ public class Translation3d implements Interpolatable { * * @return The Z component of the translation. */ + @JsonProperty public double getZ() { return m_z; }