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) {
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
index 4fd3f15..18486e3 100644
--- a/frc971/control_loops/pose_test.cc
+++ b/frc971/control_loops/pose_test.cc
@@ -83,6 +83,53 @@
EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
}
+// Tests that we can go between transformation matrices and Pose objects.
+TEST(PoseTest, TransformationMatrixTest) {
+ // First, sanity check the basic case.
+ Pose pose({0, 0, 0}, 0);
+ typedef Eigen::Matrix<double, 4, 4> TransformationMatrix;
+ ASSERT_EQ(TransformationMatrix::Identity(), pose.AsTransformationMatrix());
+ Pose reproduced_pose(pose.AsTransformationMatrix());
+ ASSERT_EQ(reproduced_pose.rel_pos(), pose.rel_pos());
+ ASSERT_EQ(reproduced_pose.rel_theta(), pose.rel_theta());
+ // Check a basic case of rotation + translation.
+ *pose.mutable_pos() << 1, 2, 3;
+ pose.set_theta(M_PI_2);
+ TransformationMatrix expected;
+ expected << 0, -1, 0, 1, 1, 0, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1;
+ TransformationMatrix pose_transformation =
+ pose.AsTransformationMatrix();
+ ASSERT_LT((expected - pose_transformation).norm(), 1e-15)
+ << "expected:\n"
+ << expected << "\nBut got:\n"
+ << pose_transformation;
+ ASSERT_EQ(Eigen::Vector4d(1, 2, 3, 1),
+ pose_transformation * Eigen::Vector4d(0, 0, 0, 1));
+ ASSERT_LT((Eigen::Vector4d(0, 3, 3, 1) -
+ pose_transformation * Eigen::Vector4d(1, 1, 0, 1))
+ .norm(),
+ 1e-15)
+ << "got " << pose_transformation * Eigen::Vector4d(1, 1, 0, 1);
+
+ // Also, confirm that setting a new base does not affect the pose.
+ Pose faux_base({1, 1, 1}, 1);
+ pose.set_base(&faux_base);
+
+ ASSERT_EQ(pose_transformation, pose.AsTransformationMatrix());
+
+ reproduced_pose = Pose(pose_transformation);
+ ASSERT_EQ(reproduced_pose.rel_pos(), pose.rel_pos());
+ ASSERT_EQ(reproduced_pose.rel_theta(), pose.rel_theta());
+ // And check that if we introduce a pitch to the transformation matrix that it
+ // does not impact the resulting Pose (which only has a yaw component).
+ pose_transformation.block<3, 3>(0, 0) =
+ Eigen::AngleAxis<double>(0.5, Eigen::Vector3d::UnitX()) *
+ pose_transformation.block<3, 3>(0, 0);
+ reproduced_pose = Pose(pose_transformation);
+ ASSERT_EQ(reproduced_pose.rel_pos(), pose.rel_pos());
+ ASSERT_EQ(reproduced_pose.rel_theta(), pose.rel_theta());
+}
+
// Tests that basic accessors for LineSegment behave as expected.
TEST(LineSegmentTest, BasicAccessorTest) {
LineSegment l;