Fix localization pose projection

crates/nodes/localization-3d/src/pose.rs

@@ -81,17 +81,18 @@ fn constrain_localization_to_ground(
     localization: Isometry3<Field, Robot>,
     ground_to_robot: &Isometry3<Ground, Robot>,
 ) -> Isometry3<Field, Robot> {
-    let (_, _, yaw) = localization.inner.rotation.euler_angles();
-    let (roll, pitch, _) = ground_to_robot.inner.rotation.euler_angles();
-
-    let mut translation = localization.inner.translation;
-    translation.vector.z = ground_to_robot.inner.translation.vector.z;
-
-    nalgebra::Isometry3::from_parts(
-        translation,
-        nalgebra::UnitQuaternion::from_euler_angles(roll, pitch, yaw),
-    )
-    .framed_transform()
+    let robot_to_field = localization.inverse();
+    let ground_to_field = robot_to_field * *ground_to_robot;
+    let (_, _, yaw) = ground_to_field.inner.rotation.euler_angles();
+    let translation = ground_to_field.inner.translation.vector;
+
+    let flattened_ground_to_field = Isometry3::from_parts(
+        linear_algebra::vector![<Field>, translation.x, translation.y, 0.0],
+        Orientation3::from_euler_angles(0.0, 0.0, yaw),
+    );
+    let constrained_robot_to_field = flattened_ground_to_field * ground_to_robot.inverse();
+
+    constrained_robot_to_field.inverse()
 }
 
 fn robot_height() -> f32 {
@@ -191,27 +192,49 @@ mod tests {
     }
 
     #[test]
-    fn constrained_localization_preserves_yaw_and_xy_but_uses_ground_tilt_and_height() {
-        let localization: Isometry3<Field, Robot> = nalgebra::Isometry3::from_parts(
-            nalgebra::Translation3::new(1.5, -2.0, -1.7),
-            nalgebra::UnitQuaternion::from_euler_angles(3.0, 0.2, 0.7),
+    fn constrained_localization_preserves_ground_pose_without_height_coupling() {
+        let robot_to_field: Isometry3<Robot, Field> = nalgebra::Isometry3::from_parts(
+            nalgebra::Translation3::new(4.0, -3.0, 0.6),
+            nalgebra::UnitQuaternion::from_euler_angles(0.2, -0.3, 0.7),
         )
         .framed_transform();
+        let localization = robot_to_field.inverse();
         let ground_to_robot: Isometry3<Ground, Robot> = nalgebra::Isometry3::from_parts(
             nalgebra::Translation3::new(0.01, -0.02, -0.523),
-            nalgebra::UnitQuaternion::from_euler_angles(-0.045, -0.047, 0.1),
+            nalgebra::UnitQuaternion::from_euler_angles(-0.045, -0.047, 0.0),
         )
         .framed_transform();
 
         let constrained = constrain_localization_to_ground(localization, &ground_to_robot);
-        let (roll, pitch, yaw) = constrained.inner.rotation.euler_angles();
-
-        assert!((constrained.inner.translation.vector.x - 1.5).abs() < 1.0e-6);
-        assert!((constrained.inner.translation.vector.y + 2.0).abs() < 1.0e-6);
-        assert!((constrained.inner.translation.vector.z + 0.523).abs() < 1.0e-6);
-        assert!((roll + 0.045).abs() < 1.0e-6);
-        assert!((pitch + 0.047).abs() < 1.0e-6);
-        assert!((yaw - 0.7).abs() < 1.0e-6);
+        let constrained_robot_to_field = constrained.inverse();
+        let constrained_ground_to_field = constrained_robot_to_field * ground_to_robot;
+        let original_ground_to_field = robot_to_field * ground_to_robot;
+        let (roll, pitch, yaw) = constrained_ground_to_field.inner.rotation.euler_angles();
+        let (_, _, original_yaw) = original_ground_to_field.inner.rotation.euler_angles();
+        let robot_to_ground = ground_to_robot.inverse();
+
+        assert!(
+            (constrained_ground_to_field.inner.translation.vector.x
+                - original_ground_to_field.inner.translation.vector.x)
+                .abs()
+                < 1.0e-6
+        );
+        assert!(
+            (constrained_ground_to_field.inner.translation.vector.y
+                - original_ground_to_field.inner.translation.vector.y)
+                .abs()
+                < 1.0e-6
+        );
+        assert!(constrained_ground_to_field.inner.translation.vector.z.abs() < 1.0e-6);
+        assert!(roll.abs() < 1.0e-6);
+        assert!(pitch.abs() < 1.0e-6);
+        assert!((yaw - original_yaw).abs() < 1.0e-6);
+        assert!(
+            (constrained_robot_to_field.inner.translation.vector.z
+                - robot_to_ground.inner.translation.vector.z)
+                .abs()
+                < 1.0e-6
+        );
     }
 
     #[test]

crates/projection/src/camera_matrix.rs

@@ -116,7 +116,7 @@ impl CameraMatrix {
         correction_in_robot: Rotation3<Robot, Robot>,
         correction_in_camera: Rotation3<Camera, Camera>,
     ) -> Self {
-        let corrected_ground_to_robot = correction_in_robot * self.ground_to_robot;
+        let corrected_ground_to_robot = self.ground_to_robot;
         let corrected_robot_to_head = self.robot_to_head * correction_in_robot;
         let corrected_head_to_camera = correction_in_camera * self.head_to_camera;
 
@@ -146,7 +146,7 @@ impl CameraMatrix {
 #[cfg(test)]
 mod tests {
     use approx::assert_relative_eq;
-    use linear_algebra::vector;
+    use linear_algebra::{Orientation3, vector};
 
     use super::*;
 
@@ -175,4 +175,41 @@ mod tests {
             Intrinsic::calculate_field_of_view(focals_scaled, image_size_abs)
         );
     }
+
+    #[test]
+    fn correction_in_robot_is_applied_once_without_changing_ground_to_robot() {
+        let ground_to_robot = Isometry3::from_parts(
+            vector![<Robot>, 0.1, -0.2, -0.5],
+            Orientation3::from_euler_angles(0.03, -0.04, 0.0),
+        );
+        let robot_to_head = Isometry3::from_translation(0.0, 0.0, 0.3);
+        let head_to_camera = Isometry3::from_translation(0.05, 0.0, 0.02);
+        let camera_matrix = CameraMatrix::from_normalized_focal_and_center(
+            nalgebra::vector![0.5, 0.5],
+            nalgebra::point![0.5, 0.5],
+            vector![640.0, 480.0],
+            ground_to_robot,
+            robot_to_head,
+            head_to_camera,
+        );
+        let correction_in_robot = Rotation3::from_euler_angles(0.1, -0.2, 0.3);
+        let correction_in_camera = Rotation3::from_euler_angles(-0.4, 0.5, -0.6);
+
+        let corrected = camera_matrix.to_corrected(correction_in_robot, correction_in_camera);
+        let expected_ground_to_camera = correction_in_camera
+            * head_to_camera
+            * robot_to_head
+            * correction_in_robot
+            * ground_to_robot;
+
+        assert_isometry_near(corrected.ground_to_robot, ground_to_robot);
+        assert_isometry_near(corrected.ground_to_camera, expected_ground_to_camera);
+    }
+
+    fn assert_isometry_near<From, To>(actual: Isometry3<From, To>, expected: Isometry3<From, To>) {
+        assert!(
+            (actual.inner.translation.vector - expected.inner.translation.vector).norm() < 1.0e-6
+        );
+        assert!(actual.inner.rotation.angle_to(&expected.inner.rotation) < 1.0e-6);
+    }
 }