Ignore camera-to-robot pitch/roll in y2020 localizer
Make it so that if camera pitch is significantly off, the lever arm from
the camera to the robot origin does not cause an offset in our position
estimates.
Also, add the superstructure to the drivetrain replay.
Change-Id: I7701e7f6a7cfc93045f76033effcd916e0c263df
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 1cab511..5169a64 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -227,8 +227,15 @@
// reading. Note that the Pose object ignores any roll/pitch components, so
// if the camera's extrinsics for pitch/roll are off, this should just
// ignore it.
- const Pose measured_pose(H_field_target *
- (H_robot_camera * H_camera_target).inverse());
+ const Pose measured_camera_pose(H_field_target * H_camera_target.inverse());
+ // Calculate the camera-to-robot transformation matrix ignoring the
+ // pitch/roll of the camera.
+ // TODO(james): This could probably be made a bit more efficient, but I
+ // don't think this is anywhere near our bottleneck currently.
+ const Eigen::Matrix<float, 4, 4> H_camera_robot_stripped =
+ Pose(H_robot_camera).AsTransformationMatrix().inverse();
+ const Pose measured_pose(measured_camera_pose.AsTransformationMatrix() *
+ H_camera_robot_stripped);
// This "Z" is the robot pose directly implied by the camera results.
// Currently, we do not actually use this result directly. However, it is
// kept around in case we want to quickly re-enable it.