Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial AprilTag support (#458)

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

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

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;