Allow translating Pose objects to/from matrices
We are currently using transformation matrices to talk about the
cameras/targets, so start adding some utilities to the Pose object to
accommodate this.
Change-Id: I8a68f6716f52e617822df51d6d9dd59dc375dc44
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 13bd6e6..20672cf 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -9,6 +9,20 @@
namespace frc971 {
namespace control_loops {
+// Constructs a homogeneous transformation matrix for rotating about the Z axis.
+template <typename Scalar>
+Eigen::Matrix<Scalar, 4, 4> TransformationMatrixForYaw(Scalar yaw) {
+ Eigen::Matrix<Scalar, 4, 4> matrix;
+ matrix.setIdentity();
+ const Scalar stheta = std::sin(yaw);
+ const Scalar ctheta = std::cos(yaw);
+ matrix(0, 0) = ctheta;
+ matrix(1, 1) = ctheta;
+ matrix(0, 1) = -stheta;
+ matrix(1, 0) = stheta;
+ return matrix;
+}
+
// Provides a representation of a transformation on the field.
// Currently, this is heavily geared towards things that occur in a 2-D plane.
// The Z-axis is rarely used (but still relevant; e.g., in 2019 some of the
@@ -41,7 +55,7 @@
// The type that contains the translational (x, y, z) component of the Pose.
typedef Eigen::Matrix<Scalar, 3, 1> Pos;
- // Provide a default constructor that crease a pose at the origin.
+ // Provide a default constructor that creates a pose at the origin.
TypedPose() : TypedPose({0.0, 0.0, 0.0}, 0.0) {}
// Construct a Pose in the absolute frame with a particular position and yaw.
@@ -54,6 +68,15 @@
TypedPose(const TypedPose<Scalar> *base, const Pos &rel_pos, Scalar rel_theta)
: base_(base), pos_(rel_pos), theta_(rel_theta) {}
+ // Constructs a Pose from a homogeneous transformation matrix. Ignores the
+ // pitch/roll components of the rotation. Ignores the bottom row.
+ TypedPose(const Eigen::Matrix<Scalar, 4, 4> &H) {
+ pos_ = H.template block<3, 1>(0, 3);
+ const Eigen::Vector3d rotated_x =
+ H.template block<3, 3>(0, 0) * Eigen::Vector3d::UnitX();
+ theta_ = std::atan2(rotated_x.y(), rotated_x.x());
+ }
+
// Calculate the current global position of this Pose.
Pos abs_pos() const {
if (base_ == nullptr) {
@@ -93,6 +116,15 @@
return abs_pos().template topRows<2>();
}
+ // Returns a transformation matrix representing this pose--note that this
+ // explicitly does not include the base position, so this is equivalent to a
+ // translation and rotation by rel_pos and rel_theta.
+ Eigen::Matrix<Scalar, 4, 4> AsTransformationMatrix() const {
+ Eigen::Matrix<Scalar, 4, 4> matrix = TransformationMatrixForYaw(theta_);
+ matrix.template block<3, 1>(0, 3) = pos_;
+ return matrix;
+ }
+
// Provide a copy of this that is set to have the same
// current absolute Pose as this, but have a different base.
// This can be used, e.g., to compute a Pose for a vision target that is
@@ -104,6 +136,14 @@
// the returned object (unless new_base == nullptr).
TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+ // Convert this pose to the heading/distance/skew numbers that we
+ // traditionally use for EKF corrections.
+ Eigen::Matrix<Scalar, 3, 1> ToHeadingDistanceSkew() const {
+ const Scalar target_heading = heading();
+ return {target_heading, xy_norm(),
+ aos::math::NormalizeAngle(rel_theta() - target_heading)};
+ }
+
private:
// A rotation-matrix like representation of the rotation for a given angle.
inline static Eigen::AngleAxis<Scalar> YawRotation(double theta) {