Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +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

159
photon-lib/src/main/native/cpp/geometry/Pose3d.cpp Normal file
View File

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

95
photon-lib/src/main/native/cpp/geometry/Quaternion.cpp Normal file
View File

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

248
photon-lib/src/main/native/cpp/geometry/Rotation3d.cpp Normal file
View File

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

60
photon-lib/src/main/native/cpp/geometry/Transform3d.cpp Normal file
View File

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

88
photon-lib/src/main/native/cpp/geometry/Translation3d.cpp Normal file
View File

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

29
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
View File

@@ -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++) {

18
photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp
View File

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

43
photon-lib/src/main/native/include/frc/EigenCore.h Normal file
View File

@@ -0,0 +1,43 @@
/*
* 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.
*/
#pragma once
#include <Eigen/Core>
namespace frc {
template <int Size>
using Vectord = Eigen::Vector<double, Size>;
template <int Rows, int Cols,
int Options = Eigen::AutoAlign |
((Rows == 1 && Cols != 1) ? Eigen::RowMajor
: (Cols == 1 && Rows != 1)
? Eigen::ColMajor
: EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
int MaxRows = Rows, int MaxCols = Cols>
using Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
} // namespace frc

200
photon-lib/src/main/native/include/frc/geometry/Pose3d.h Normal file
View File

@@ -0,0 +1,200 @@
/*
* 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.
*/
#pragma once
#include <frc/geometry/Pose2d.h>
#include <wpi/SymbolExports.h>
#include "Transform3d.h"
#include "Translation3d.h"
#include "Twist3d.h"
namespace frc {
/**
* Represents a 3D pose containing translational and rotational elements.
*/
class WPILIB_DLLEXPORT Pose3d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
*/
constexpr Pose3d() = default;
/**
* 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.
*/
Pose3d(Translation3d translation, Rotation3d 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.
*/
Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
/**
* 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.
*/
Pose3d operator+(const Transform3d& other) const;
/**
* 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.
*/
Transform3d operator-(const Pose3d& other) const;
/**
* Checks equality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Pose3d& other) const;
/**
* Checks inequality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Pose3d& other) const;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation3d& Rotation() const { 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.
*/
Pose3d TransformBy(const Transform3d& other) const;
/**
* Returns the other pose relative to the current pose.
*
* 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.
*/
Pose3d RelativeTo(const Pose3d& other) const;
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* 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.
*
* "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_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
* 0.5_deg}}.
*
* @return The new pose of the robot.
*/
Pose3d Exp(const Twist3d& twist) const;
/**
* 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.
*/
Twist3d Log(const Pose3d& end) const;
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/
Pose2d ToPose2d() const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

114
photon-lib/src/main/native/include/frc/geometry/Quaternion.h Normal file
View File

@@ -0,0 +1,114 @@
/*
* 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.
*/
#pragma once
#include <frc/EigenCore.h>
#include <wpi/SymbolExports.h>
namespace frc {
class WPILIB_DLLEXPORT Quaternion {
public:
/**
* Constructs a quaternion with a default angle of 0 degrees.
*/
Quaternion() = default;
/**
* 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.
*/
Quaternion(double w, double x, double y, double z);
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
*/
Quaternion operator*(const Quaternion& other) const;
/**
* Checks equality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Quaternion& other) const;
/**
* Checks inequality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Quaternion& other) const;
/**
* Returns the inverse of the quaternion.
*/
Quaternion Inverse() const;
/**
* Normalizes the quaternion.
*/
Quaternion Normalize() const;
/**
* Returns W component of the quaternion.
*/
double W() const;
/**
* Returns X component of the quaternion.
*/
double X() const;
/**
* Returns Y component of the quaternion.
*/
double Y() const;
/**
* Returns Z component of the quaternion.
*/
double Z() const;
/**
* Returns the rotation vector representation of this quaternion.
*
* This is also the log operator of SO(3).
*/
Eigen::Vector3d ToRotationVector() const;
private:
double m_r = 1.0;
Eigen::Vector3d m_v{0.0, 0.0, 0.0};
};
} // namespace frc

193
photon-lib/src/main/native/include/frc/geometry/Rotation3d.h Normal file
View File

@@ -0,0 +1,193 @@
/*
* 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.
*/
#pragma once
#include <frc/EigenCore.h>
#include <frc/geometry/Rotation2d.h>
#include <units/angle.h>
#include <wpi/SymbolExports.h>
#include "Quaternion.h"
namespace frc {
/**
* A rotation in a 3D coordinate frame represented by a quaternion.
*/
class WPILIB_DLLEXPORT Rotation3d {
public:
/**
* Constructs a Rotation3d with a default angle of 0 degrees.
*/
Rotation3d() = default;
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
explicit Rotation3d(const Quaternion& q);
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* Extrinsic rotations occur in that order around the axes in the fixed global
* frame rather than the body frame.
*
* @param roll The counterclockwise rotation angle around the X axis (roll).
* @param pitch The counterclockwise rotation angle around the Y axis (pitch).
* @param yaw The counterclockwise rotation angle around the Z axis (yaw).
*/
Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis
* doesn't have to be normalized.
*
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
Rotation3d(const Vectord<3>& axis, units::radian_t angle);
/**
* Constructs a Rotation3d from a rotation matrix.
*
* @param rotationMatrix The rotation matrix.
* @throws std::domain_error if the rotation matrix isn't special orthogonal.
*/
explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
/**
* Constructs a Rotation3d that rotates the initial vector onto the final
* vector.
*
* This is useful for turning a 3D vector (final) into an orientation relative
* to a coordinate system vector (initial).
*
* @param initial The initial vector.
* @param final The final vector.
*/
Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
/**
* Adds two rotations together.
*
* @param other The rotation to add.
*
* @return The sum of the two rotations.
*/
Rotation3d operator+(const Rotation3d& other) const;
/**
* 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.
*/
Rotation3d operator-(const Rotation3d& other) const;
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
Rotation3d operator-() const;
/**
* Multiplies the current rotation by a scalar.
* @param scalar The scalar.
*
* @return The new scaled Rotation3d.
*/
Rotation3d operator*(double scalar) const;
/**
* Checks equality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Rotation3d& other) const;
/**
* Checks inequality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Rotation3d& other) const;
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
*
* @return The new rotated Rotation3d.
*/
Rotation3d RotateBy(const Rotation3d& other) const;
/**
* Returns the quaternion representation of the Rotation3d.
*/
const Quaternion& GetQuaternion() const;
/**
* Returns the counterclockwise rotation angle around the X axis (roll).
*/
units::radian_t X() const;
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch).
*/
units::radian_t Y() const;
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw).
*/
units::radian_t Z() const;
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
Vectord<3> Axis() const;
/**
* Returns the angle in the axis-angle representation of this rotation.
*/
units::radian_t Angle() const;
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.
*/
Rotation2d ToRotation2d() const;
private:
Quaternion m_q;
};
} // namespace frc

141
photon-lib/src/main/native/include/frc/geometry/Transform3d.h Normal file
View File

@@ -0,0 +1,141 @@
/*
* 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.
*/
#pragma once
#include <wpi/SymbolExports.h>
#include "Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
/**
* Represents a transformation for a Pose3d.
*/
class WPILIB_DLLEXPORT Transform3d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform3d(Pose3d initial, Pose3d final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform3d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform3d Inverse() const;
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator*(double scalar) const {
return Transform3d(m_translation * scalar, m_rotation * scalar);
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform3d operator+(const Transform3d& other) const;
/**
* Checks equality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Transform3d& other) const;
/**
* Checks inequality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Transform3d& other) const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

205
photon-lib/src/main/native/include/frc/geometry/Translation3d.h Normal file
View File

@@ -0,0 +1,205 @@
/*
* 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.
*/
#pragma once
#include <frc/geometry/Translation2d.h>
#include <units/length.h>
#include <wpi/SymbolExports.h>
#include "Rotation3d.h"
namespace frc {
/**
* Represents a translation in 3D space.
* This object can be used to represent a point or a vector.
*
* 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.
*/
class WPILIB_DLLEXPORT Translation3d {
public:
/**
* Constructs a Translation3d with X, Y, and Z components equal to zero.
*/
constexpr Translation3d() = default;
/**
* 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.
*/
Translation3d(units::meter_t x, units::meter_t y, units::meter_t 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.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
/**
* Calculates the distance between two translations in 3D space.
*
* The distance between translations is defined as
* √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
*
* @param other The translation to compute the distance to.
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const;
/**
* Returns the X component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t Z() const { return m_z; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 3D space.
*
* 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.
*/
Translation3d RotateBy(const Rotation3d& other) const;
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
Translation2d ToTranslation2d() const;
/**
* Returns the sum of two translations in 3D space.
*
* 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.
*/
Translation3d operator+(const Translation3d& other) const;
/**
* Returns the difference between two translations.
*
* 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.
*/
Translation3d operator-(const Translation3d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
* negating all components of the translation.
*
* @return The inverse of the current translation.
*/
Translation3d operator-() const;
/**
* Returns the translation multiplied by a scalar.
*
* 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.
*/
Translation3d operator*(double scalar) const;
/**
* Returns the translation divided by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
* 2.25}.
*
* @param scalar The scalar to divide by.
*
* @return The scaled translation.
*/
Translation3d operator/(double scalar) const;
/**
* Checks equality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation3d& other) const;
/**
* Checks inequality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Translation3d& other) const;
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
units::meter_t m_z = 0_m;
};
} // namespace frc

106
photon-lib/src/main/native/include/frc/geometry/Twist3d.h Normal file
View File

@@ -0,0 +1,106 @@
/*
* 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.
*/
#pragma once
#include <frc/geometry/Rotation3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/math.h>
#include <wpi/SymbolExports.h>
namespace frc {
/**
* 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.
*
* A Twist can be used to represent a difference between two poses.
*/
struct WPILIB_DLLEXPORT Twist3d {
/**
* Linear "dx" component
*/
units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
units::meter_t dy = 0_m;
/**
* Linear "dz" component
*/
units::meter_t dz = 0_m;
/**
* Rotation vector x component.
*/
units::radian_t rx = 0_rad;
/**
* Rotation vector y component.
*/
units::radian_t ry = 0_rad;
/**
* Rotation vector z component.
*/
units::radian_t rz = 0_rad;
/**
* Checks equality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist3d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dz - other.dz) < 1E-9_m &&
units::math::abs(rx - other.rx) < 1E-9_rad &&
units::math::abs(ry - other.ry) < 1E-9_rad &&
units::math::abs(rz - other.rz) < 1E-9_rad;
}
/**
* Checks inequality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist3d& other) const { return !operator==(other); }
/**
* Scale this by a given factor.
*
* @param factor The factor by which to scale.
* @return The scaled Twist3d.
*/
Twist3d operator*(double factor) const {
return Twist3d{dx * factor, dy * factor, dz * factor,
rx * factor, ry * factor, rz * factor};
}
};
} // namespace frc

11
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
View File

@@ -29,7 +29,7 @@
#include <utility>
#include <vector>
#include <frc/geometry/Transform2d.h>
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
@@ -55,8 +55,8 @@ class PhotonTrackedTarget {
* @Param corners The corners of the bounding rectangle.
*/
PhotonTrackedTarget(
double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose,
double yaw, double pitch, double area, double skew, int fiducialID,
const frc::Transform3d& pose,
const wpi::SmallVector<std::pair<double, double>, 4> corners);
/**
@@ -94,7 +94,7 @@ class PhotonTrackedTarget {
* Returns the pose of the target relative to the robot.
* @return The pose of the target relative to the robot.
*/
frc::Transform2d GetCameraRelativePose() const { return cameraToTarget; }
frc::Transform3d GetCameraToTarget() const { return cameraToTarget; }
bool operator==(const PhotonTrackedTarget& other) const;
bool operator!=(const PhotonTrackedTarget& other) const;
@@ -107,7 +107,8 @@ class PhotonTrackedTarget {
double pitch = 0;
double area = 0;
double skew = 0;
frc::Transform2d cameraToTarget;
int fiducialId;
frc::Transform3d cameraToTarget;
wpi::SmallVector<std::pair<double, double>, 4> corners;
};
} // namespace photonlib

4
photon-lib/src/main/native/include/photonlib/SimVisionSystem.h
View File

@@ -45,7 +45,6 @@ namespace photonlib {
class SimVisionSystem {
public:
explicit 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,
@@ -53,11 +52,10 @@ class SimVisionSystem {
void AddSimVisionTarget(SimVisionTarget tgt);
void MoveCamera(frc::Transform2d newcameraToRobot,
units::meter_t newCamHeight, units::degree_t newCamPitch);
units::meter_t newCamHeight);
void ProcessFrame(frc::Pose2d robotPose);
private:
units::degree_t camPitch;
frc::Transform2d cameraToRobot;
units::meter_t cameraHeightOffGround;
units::meter_t maxLEDRange;