Swap naming order of transformation matrices
Jim pointed out that by swapping the convention we could get easier to
read code.
Change-Id: Id81b56c3a6fd17e7d538a99f285fb2c4079fa442
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index a9e454a..5551179 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -111,9 +111,9 @@
FlatbufferToTransformationMatrix(
*result.camera_calibration()->fixed_extrinsics());
// Calculate the pose of the camera relative to the robot origin.
- Eigen::Matrix<double, 4, 4> H_camera_robot = fixed_extrinsics;
+ Eigen::Matrix<double, 4, 4> H_robot_camera = fixed_extrinsics;
if (is_turret) {
- H_camera_robot = H_camera_robot *
+ H_robot_camera = H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw(
turret_data.position) *
FlatbufferToTransformationMatrix(
@@ -130,14 +130,14 @@
!vision_result->has_field_to_target()) {
continue;
}
- const Eigen::Matrix<double, 4, 4> H_target_camera =
+ const Eigen::Matrix<double, 4, 4> H_camera_target =
FlatbufferToTransformationMatrix(*vision_result->camera_to_target());
- const Eigen::Matrix<double, 4, 4> H_target_field =
+ const Eigen::Matrix<double, 4, 4> H_field_target =
FlatbufferToTransformationMatrix(*vision_result->field_to_target());
// Back out the robot position that is implied by the current camera
// reading.
- const Pose measured_pose(H_target_field *
- (H_camera_robot * H_target_camera).inverse());
+ const Pose measured_pose(H_field_target *
+ (H_robot_camera * H_camera_target).inverse());
const Eigen::Matrix<double, 3, 1> Z(measured_pose.rel_pos().x(),
measured_pose.rel_pos().y(),
measured_pose.rel_theta());
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 3be86da..acab856 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -228,26 +228,27 @@
drivetrain_plant_.state()(2, 0));
std::unique_ptr<ImageMatchResultT> frame(new ImageMatchResultT());
- for (const auto &H_target_field : TargetLocations()) {
+ for (const auto &H_field_target : TargetLocations()) {
std::unique_ptr<CameraPoseT> camera_target(new CameraPoseT());
camera_target->field_to_target.reset(new TransformationMatrixT());
- camera_target->field_to_target->data = MatrixToVector(H_target_field);
+ camera_target->field_to_target->data = MatrixToVector(H_field_target);
- Eigen::Matrix<double, 4, 4> H_camera_turret =
+ Eigen::Matrix<double, 4, 4> H_turret_camera =
Eigen::Matrix<double, 4, 4>::Identity();
if (is_turreted_) {
- H_camera_turret = frc971::control_loops::TransformationMatrixForYaw(
+ H_turret_camera = frc971::control_loops::TransformationMatrixForYaw(
turret_position_) *
CameraTurretTransformation();
}
// 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_camera_turret).inverse() *
- H_target_field);
+ camera_target->camera_to_target->data =
+ MatrixToVector((robot_pose.AsTransformationMatrix() *
+ TurretRobotTransformation() * H_turret_camera)
+ .inverse() *
+ H_field_target);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -260,17 +261,17 @@
{
frame->camera_calibration->fixed_extrinsics.reset(
new TransformationMatrixT());
- TransformationMatrixT *H_turret_robot =
+ TransformationMatrixT *H_robot_turret =
frame->camera_calibration->fixed_extrinsics.get();
- H_turret_robot->data = MatrixToVector(TurretRobotTransformation());
+ H_robot_turret->data = MatrixToVector(TurretRobotTransformation());
}
if (is_turreted_) {
frame->camera_calibration->turret_extrinsics.reset(
new TransformationMatrixT());
- TransformationMatrixT *H_camera_turret =
+ TransformationMatrixT *H_turret_camera =
frame->camera_calibration->turret_extrinsics.get();
- H_camera_turret->data = MatrixToVector(CameraTurretTransformation());
+ H_turret_camera->data = MatrixToVector(CameraTurretTransformation());
}
camera_delay_queue_.emplace(monotonic_now(), std::move(frame));