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_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index a3e26b5..ab3b075 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -286,11 +286,11 @@
// TODO(james): Use non-zero turret angles.
camera_target->camera_to_target.reset(new TransformationMatrixT());
- camera_target->camera_to_target->data =
- MatrixToVector((robot_pose.AsTransformationMatrix() *
- TurretRobotTransformation() * H_turret_camera)
- .inverse() *
- H_field_target);
+ camera_target->camera_to_target->data = MatrixToVector(
+ (robot_pose.AsTransformationMatrix() * TurretRobotTransformation() *
+ H_turret_camera * camera_calibration_offset_)
+ .inverse() *
+ H_field_target);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -366,6 +366,11 @@
std::unique_ptr<ImageMatchResultT>>>
camera_delay_queue_;
+ // Offset to add to the camera for actually taking the images, to simulate an
+ // inaccurate calibration.
+ Eigen::Matrix<double, 4, 4> camera_calibration_offset_ =
+ Eigen::Matrix<double, 4, 4>::Identity();
+
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
void set_camera_is_turreted(bool turreted) { is_turreted_ = turreted; }
@@ -453,6 +458,24 @@
EXPECT_TRUE(VerifyEstimatorAccurate(2e-3));
}
+// Tests that camera updates with a perfect model but incorrect camera pitch
+// results in no errors.
+TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdateWithBadPitch) {
+ // Introduce some camera pitch.
+ camera_calibration_offset_.template block<3, 3>(0, 0) =
+ Eigen::AngleAxis<double>(0.1, Eigen::Vector3d::UnitY())
+ .toRotationMatrix();
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(3));
+ VerifyNearGoal();
+ EXPECT_TRUE(VerifyEstimatorAccurate(2e-3));
+}
+
// Tests that camera updates with a constant initial error in the position
// results in convergence.
TEST_F(LocalizedDrivetrainTest, InitialPositionError) {