Initial AprilTag support (#458)

(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>

photon-lib/src/main/native/cpp/geometry/Pose3d.cpp

@@ -0,0 +1,159 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <cmath>
+
+#include <frc/geometry/Pose3d.h>
+
+using namespace frc;
+
+namespace {
+
+/**
+ * Applies the hat operator to a rotation vector.
+ *
+ * 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.
+ */
+Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+  // Given a rotation vector <a, b, c>,
+  //         [ 0 -c  b]
+  // Omega = [ c  0 -a]
+  //         [-b  a  0]
+  return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
+                       {rotation(2), 0.0, -rotation(0)},
+                       {-rotation(1), rotation(0), 0.0}};
+}
+}  // namespace
+
+Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+               Rotation3d rotation)
+    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
+Pose3d Pose3d::operator+(const Transform3d& other) const {
+  return TransformBy(other);
+}
+
+Transform3d Pose3d::operator-(const Pose3d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform3d{pose.Translation(), pose.Rotation()};
+}
+
+bool Pose3d::operator==(const Pose3d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Pose3d::operator!=(const Pose3d& other) const {
+  return !operator==(other);
+}
+
+Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          m_rotation + other.Rotation()};
+}
+
+Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
+  const Transform3d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+Pose3d Pose3d::Exp(const Twist3d& twist) const {
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(
+      Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  double thetaSq =
+      (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
+
+  // Get left Jacobian of SO3. See first line in right column of
+  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+  Matrixd<3, 3> J;
+  if (thetaSq < 1E-9 * 1E-9) {
+    // V = I + 0.5ω
+    J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+  } else {
+    double theta = std::sqrt(thetaSq);
+    // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
+    J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
+        (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+  }
+
+  // Get translation component
+  Vectord<3> translation =
+      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+
+  const Transform3d transform{Translation3d{units::meter_t{translation(0)},
+                                            units::meter_t{translation(1)},
+                                            units::meter_t{translation(2)}},
+                              Rotation3d{twist.rx, twist.ry, twist.rz}};
+
+  return *this + transform;
+}
+
+Twist3d Pose3d::Log(const Pose3d& end) const {
+  const auto transform = end.RelativeTo(*this);
+
+  Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  double thetaSq = rotVec.squaredNorm();
+
+  // Get left Jacobian inverse of SO3. See fourth line in right column of
+  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
+  Matrixd<3, 3> Jinv;
+  if (thetaSq < 1E-9 * 1E-9) {
+    // J⁻¹ = I − 0.5ω + 1/12 ω²
+    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+  } else {
+    double theta = std::sqrt(thetaSq);
+    double halfTheta = 0.5 * theta;
+
+    // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
+    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
+           (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
+               thetaSq * OmegaSq;
+  }
+
+  // Get dtranslation component
+  Vectord<3> dtranslation =
+      Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
+                        transform.Z().value()};
+
+  return Twist3d{
+      units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
+      units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
+      units::radian_t{rotVec(1)},      units::radian_t{rotVec(2)}};
+}
+
+Pose2d Pose3d::ToPose2d() const {
+  return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
+}

photon-lib/src/main/native/cpp/geometry/Quaternion.cpp

@@ -0,0 +1,95 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <frc/geometry/Quaternion.h>
+
+using namespace frc;
+
+Quaternion::Quaternion(double w, double x, double y, double z)
+    : m_r{w}, m_v{x, y, z} {}
+
+Quaternion Quaternion::operator*(const Quaternion& other) const {
+  // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+  const auto& r1 = m_r;
+  const auto& v1 = m_v;
+  const auto& r2 = other.m_r;
+  const auto& v2 = other.m_v;
+
+  // v₁ x v₂
+  Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
+                        v2(0) * v1(2) - v1(0) * v2(2),
+                        v1(0) * v2(1) - v2(0) * v1(1)};
+
+  // v = r₁v₂ + r₂v₁ + v₁ x v₂
+  Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
+
+  return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+}
+
+bool Quaternion::operator==(const Quaternion& other) const {
+  return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+}
+
+bool Quaternion::operator!=(const Quaternion& other) const {
+  return !operator==(other);
+}
+
+Quaternion Quaternion::Inverse() const {
+  return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+}
+
+Quaternion Quaternion::Normalize() const {
+  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  if (norm == 0.0) {
+    return Quaternion{};
+  } else {
+    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+  }
+}
+
+double Quaternion::W() const { return m_r; }
+
+double Quaternion::X() const { return m_v(0); }
+
+double Quaternion::Y() const { return m_v(1); }
+
+double Quaternion::Z() const { return m_v(2); }
+
+Eigen::Vector3d Quaternion::ToRotationVector() const {
+  // 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 (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
+  } else {
+    if (W() < 0.0) {
+      return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
+    } else {
+      return 2.0 * std::atan2(norm, W()) / norm * m_v;
+    }
+  }
+}

photon-lib/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -0,0 +1,248 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <cmath>
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/QR>
+#include <frc/fmt/Eigen.h>
+#include <frc/geometry/Rotation3d.h>
+#include <units/math.h>
+#include <wpi/numbers>
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+Rotation3d::Rotation3d(const Quaternion& q) { m_q = q.Normalize(); }
+
+Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
+                       units::radian_t yaw) {
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+  double cr = units::math::cos(roll * 0.5);
+  double sr = units::math::sin(roll * 0.5);
+
+  double cp = units::math::cos(pitch * 0.5);
+  double sp = units::math::sin(pitch * 0.5);
+
+  double cy = units::math::cos(yaw * 0.5);
+  double sy = units::math::sin(yaw * 0.5);
+
+  m_q = 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};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+  double norm = axis.norm();
+  if (norm == 0.0) {
+    return;
+  }
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+  Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+  m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
+}
+
+Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
+  const auto& R = rotationMatrix;
+
+  // Require that the rotation matrix is special orthogonal. This is true if the
+  // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+  if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
+    std::string msg =
+        fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::domain_error(msg);
+  }
+  if (R.determinant() != 1.0) {
+    std::string msg = fmt::format(
+        "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
+        R);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::domain_error(msg);
+  }
+
+  // Turn rotation matrix into a quaternion
+  // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+  double trace = R(0, 0) + R(1, 1) + R(2, 2);
+  double w;
+  double x;
+  double y;
+  double z;
+
+  if (trace > 0.0) {
+    double s = 0.5 / std::sqrt(trace + 1.0);
+    w = 0.25 / s;
+    x = (R(2, 1) - R(1, 2)) * s;
+    y = (R(0, 2) - R(2, 0)) * s;
+    z = (R(1, 0) - R(0, 1)) * s;
+  } else {
+    if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+      w = (R(2, 1) - R(1, 2)) / s;
+      x = 0.25 * s;
+      y = (R(0, 1) + R(1, 0)) / s;
+      z = (R(0, 2) + R(2, 0)) / s;
+    } else if (R(1, 1) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+      w = (R(0, 2) - R(2, 0)) / s;
+      x = (R(0, 1) + R(1, 0)) / s;
+      y = 0.25 * s;
+      z = (R(1, 2) + R(2, 1)) / s;
+    } else {
+      double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+      w = (R(1, 0) - R(0, 1)) / s;
+      x = (R(0, 2) + R(2, 0)) / s;
+      y = (R(1, 2) + R(2, 1)) / s;
+      z = 0.25 * s;
+    }
+  }
+
+  m_q = Quaternion{w, x, y, z};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
+  double dot = initial.dot(final);
+  double normProduct = initial.norm() * final.norm();
+  double dotNorm = dot / normProduct;
+
+  if (dotNorm > 1.0 - 1E-9) {
+    // If the dot product is 1, the two vectors point in the same direction so
+    // there's no rotation. The default initialization of m_q will work.
+    return;
+  } else if (dotNorm < -1.0 + 1E-9) {
+    // If the dot product is -1, the two vectors point in opposite directions so
+    // a 180 degree rotation is required. Any orthogonal vector can be used for
+    // it. Q in the QR decomposition is an orthonormal basis, so it contains
+    // orthogonal unit vectors.
+    Eigen::Matrix<double, 3, 1> X = initial;
+    Eigen::HouseholderQR<decltype(X)> qr{X};
+    Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
+
+    // w = std::cos(θ/2) = std::cos(90°) = 0
+    //
+    // For x, y, and z, we use the second column of Q because the first is
+    // parallel instead of orthogonal. The third column would also work.
+    m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
+  } else {
+    // initial x final
+    Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
+                         final(0) * initial(2) - initial(0) * final(2),
+                         initial(0) * final(1) - final(0) * initial(1)};
+
+    // https://stackoverflow.com/a/11741520
+    m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
+  }
+}
+
+Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
+  return RotateBy(other);
+}
+
+Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
+  return *this + -other;
+}
+
+Rotation3d Rotation3d::operator-() const { return Rotation3d{m_q.Inverse()}; }
+
+Rotation3d Rotation3d::operator*(double scalar) const {
+  // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+  if (m_q.W() >= 0.0) {
+    return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
+  } else {
+    return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
+  }
+}
+
+bool Rotation3d::operator==(const Rotation3d& other) const {
+  return m_q == other.m_q;
+}
+
+bool Rotation3d::operator!=(const Rotation3d& other) const {
+  return !operator==(other);
+}
+
+Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
+  return Rotation3d{other.m_q * m_q};
+}
+
+const Quaternion& Rotation3d::GetQuaternion() const { return m_q; }
+
+units::radian_t Rotation3d::X() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  return units::radian_t{
+      std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+}
+
+units::radian_t Rotation3d::Y() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // 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 (std::abs(ratio) >= 1.0) {
+    return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)};
+  } else {
+    return units::radian_t{std::asin(ratio)};
+  }
+}
+
+units::radian_t Rotation3d::Z() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  double y = m_q.Y();
+  double z = m_q.Z();
+
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  return units::radian_t{
+      std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+}
+
+Vectord<3> Rotation3d::Axis() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  if (norm == 0.0) {
+    return {0.0, 0.0, 0.0};
+  } else {
+    return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
+  }
+}
+
+units::radian_t Rotation3d::Angle() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
+}
+
+Rotation2d Rotation3d::ToRotation2d() const { return Rotation2d{Z()}; }

photon-lib/src/main/native/cpp/geometry/Transform3d.cpp

@@ -0,0 +1,60 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Transform3d.h>
+
+using namespace frc;
+
+Transform3d::Transform3d(Pose3d initial, Pose3d final) {
+  // 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 = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Transform3d Transform3d::Inverse() const {
+  // 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 Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+Transform3d Transform3d::operator+(const Transform3d& other) const {
+  return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
+}
+
+bool Transform3d::operator==(const Transform3d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Transform3d::operator!=(const Transform3d& other) const {
+  return !operator==(other);
+}

photon-lib/src/main/native/cpp/geometry/Translation3d.cpp

@@ -0,0 +1,88 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <frc/geometry/Translation3d.h>
+#include <units/math.h>
+
+using namespace frc;
+
+Translation3d::Translation3d(units::meter_t x, units::meter_t y,
+                             units::meter_t z)
+    : m_x(x), m_y(y), m_z(z) {}
+
+Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
+  auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
+  m_x = rectangular.X();
+  m_y = rectangular.Y();
+  m_z = rectangular.Z();
+}
+
+units::meter_t Translation3d::Distance(const Translation3d& other) const {
+  return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
+                           units::math::pow<2>(other.m_y - m_y) +
+                           units::math::pow<2>(other.m_z - m_z));
+}
+
+units::meter_t Translation3d::Norm() const {
+  return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+}
+
+Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
+  Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
+  auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
+  return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
+                       units::meter_t{qprime.Z()}};
+}
+
+Translation2d Translation3d::ToTranslation2d() const {
+  return Translation2d{m_x, m_y};
+}
+
+Translation3d Translation3d::operator+(const Translation3d& other) const {
+  return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+}
+
+Translation3d Translation3d::operator-(const Translation3d& other) const {
+  return *this + -other;
+}
+
+Translation3d Translation3d::operator-() const { return {-m_x, -m_y, -m_z}; }
+
+Translation3d Translation3d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y, scalar * m_z};
+}
+
+Translation3d Translation3d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+bool Translation3d::operator==(const Translation3d& other) const {
+  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+         units::math::abs(m_y - other.m_y) < 1E-9_m &&
+         units::math::abs(m_z - other.m_z) < 1E-9_m;
+}
+
+bool Translation3d::operator!=(const Translation3d& other) const {
+  return !operator==(other);
+}

photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp

@@ -33,13 +33,14 @@
 namespace photonlib {
 
 PhotonTrackedTarget::PhotonTrackedTarget(
-    double yaw, double pitch, double area, double skew,
-    const frc::Transform2d& pose,
+    double yaw, double pitch, double area, double skew, int id,
+    const frc::Transform3d& pose,
     const wpi::SmallVector<std::pair<double, double>, 4> corners)
     : yaw(yaw),
       pitch(pitch),
       area(area),
       skew(skew),
+      fiducialId(id),
       cameraToTarget(pose),
       corners(corners) {}
 
@@ -55,9 +56,13 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
 
 Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
   packet << target.yaw << target.pitch << target.area << target.skew
-         << target.cameraToTarget.Translation().X().value()
+         << target.fiducialId << target.cameraToTarget.Translation().X().value()
          << target.cameraToTarget.Translation().Y().value()
-         << target.cameraToTarget.Rotation().Degrees().value();
+         << target.cameraToTarget.Translation().Z().value()
+         << target.cameraToTarget.Rotation().GetQuaternion().W()
+         << target.cameraToTarget.Rotation().GetQuaternion().X()
+         << target.cameraToTarget.Rotation().GetQuaternion().Y()
+         << target.cameraToTarget.Rotation().GetQuaternion().Z();
 
   for (int i = 0; i < 4; i++) {
     packet << target.corners[i].first << target.corners[i].second;
@@ -67,15 +72,19 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
 }
 
 Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
-  packet >> target.yaw >> target.pitch >> target.area >> target.skew;
+  packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
+      target.fiducialId;
   double x = 0;
   double y = 0;
-  double rot = 0;
-  packet >> x >> y >> rot;
+  double z = 0;
+  double w = 0;
+  packet >> x >> y >> z;
+  const auto translation = frc::Translation3d(
+      units::meter_t(x), units::meter_t(y), units::meter_t(z));
+  packet >> w >> x >> y >> z;
+  const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
 
-  target.cameraToTarget =
-      frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
-                       units::degree_t(rot));
+  target.cameraToTarget = frc::Transform3d(translation, rotation);
 
   target.corners.clear();
   for (int i = 0; i < 4; i++) {

photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp

@@ -34,13 +34,11 @@ namespace photonlib {
 
 SimVisionSystem::SimVisionSystem(const std::string& name,
                                  units::degree_t camDiagFOV,
-                                 units::degree_t camPitch,
                                  frc::Transform2d cameraToRobot,
                                  units::meter_t cameraHeightOffGround,
                                  units::meter_t maxLEDRange, int cameraResWidth,
                                  int cameraResHeight, double minTargetArea)
-    : camPitch(camPitch),
-      cameraToRobot(cameraToRobot),
+    : cameraToRobot(cameraToRobot),
       cameraHeightOffGround(cameraHeightOffGround),
       maxLEDRange(maxLEDRange),
       cameraResWidth(cameraResWidth),
@@ -59,11 +57,9 @@ void SimVisionSystem::AddSimVisionTarget(SimVisionTarget tgt) {
 }
 
 void SimVisionSystem::MoveCamera(frc::Transform2d newCameraToRobot,
-                                 units::meter_t newCamHeight,
-                                 units::degree_t newCamPitch) {
+                                 units::meter_t newCamHeight) {
   cameraToRobot = newCameraToRobot;
   cameraHeightOffGround = newCamHeight;
-  camPitch = newCamPitch;
 }
 
 void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
@@ -89,11 +85,17 @@ void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
     units::degree_t yawAngle = -units::math::atan2(
         camToTargetTrans.Translation().Y(), camToTargetTrans.Translation().X());
     units::degree_t pitchAngle =
-        units::math::atan2(distVertical, distAlongGround) - camPitch;
+        units::math::atan2(distVertical, distAlongGround);
+
+    auto translation = frc::Translation3d(camToTargetTrans.Translation().X(),
+                                          camToTargetTrans.Translation().Y(),
+                                          units::meter_t(0));  // TODO z height
+    auto rotation = frc::Rotation3d(units::radian_t(0), pitchAngle, -yawAngle);
+    frc::Transform3d camToTarget3d{translation, rotation};
 
     if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
       PhotonTrackedTarget newTgt = PhotonTrackedTarget(
-          yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans,
+          yawAngle.value(), pitchAngle.value(), area, 0.0, -1, camToTarget3d,
           {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
       visibleTgtList.push_back(newTgt);
     }