💀

Author: Superbro525Alt

vision/lib/fuel_estimator.py

@@ -69,7 +69,9 @@ def rot_z(deg: float) -> np.ndarray:
 
 
 def R_field_from_cam_ypr(yaw_deg: float, pitch_deg: float, roll_deg: float) -> np.ndarray:
-    return rot_z(yaw_deg) @ rot_y(pitch_deg) @ rot_x(roll_deg)
+    # Use a camera-centric pitch convention where negative values tilt the
+    # camera downward toward the floor.
+    return rot_z(yaw_deg) @ rot_y(-pitch_deg) @ rot_x(roll_deg)
 
 
 def bbox_center_u(b: YoloBBox) -> float:
@@ -110,7 +112,7 @@ def apply_axis_conversion(ray_cv: np.ndarray, A_cam_from_cv: Optional[np.ndarray
 def ray_camera_to_field(pose: CameraPoseField, ray_c: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
     R = np.asarray(pose.R_f_c, dtype=np.float64).reshape(3, 3)
     p = np.asarray(pose.p_f, dtype=np.float64).reshape(3)
-    d_f = R.T @ np.asarray(ray_c, dtype=np.float64).reshape(3)
+    d_f = R @ np.asarray(ray_c, dtype=np.float64).reshape(3)
     d_f = d_f / float(np.linalg.norm(d_f))
     return p, d_f
 

vision/tests/test_fuel.py

@@ -231,7 +231,7 @@ def test_estimator_matches_manual_grid_with_frac(self):
 
         p_f = np.array([2.0, 1.0, 0.7], dtype=np.float64)
         pose = CameraPoseField(
-            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=30.0, roll_deg=0.0),
+            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=-30.0, roll_deg=0.0),
             p_f=p_f,
         )
 
@@ -271,7 +271,7 @@ def test_estimator_deterministic_same_inputs(self):
         A_cam_from_cv = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], dtype=np.float64)
 
         pose = CameraPoseField(
-            R_f_c=R_field_from_cam_ypr(yaw_deg=12.0, pitch_deg=25.0, roll_deg=-3.0),
+            R_f_c=R_field_from_cam_ypr(yaw_deg=12.0, pitch_deg=-25.0, roll_deg=-3.0),
             p_f=np.array([2.0, -1.0, 0.9], dtype=np.float64),
         )
 
@@ -290,7 +290,7 @@ def test_estimator_invariance_to_bbox_width_same_center(self):
 
         A_cam_from_cv = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], dtype=np.float64)
         pose = CameraPoseField(
-            R_f_c=R_field_from_cam_ypr(yaw_deg=10.0, pitch_deg=25.0, roll_deg=-5.0),
+            R_f_c=R_field_from_cam_ypr(yaw_deg=10.0, pitch_deg=-25.0, roll_deg=-5.0),
             p_f=np.array([2.0, 1.0, 0.7], dtype=np.float64),
         )
 
@@ -322,7 +322,7 @@ def test_estimator_changes_with_bbox_height_when_frac_not_zero(self):
 
         A_cam_from_cv = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], dtype=np.float64)
         pose = CameraPoseField(
-            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=30.0, roll_deg=0.0),
+            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=-30.0, roll_deg=0.0),
             p_f=np.array([2.0, 1.0, 0.7], dtype=np.float64),
         )
 
@@ -348,7 +348,7 @@ def test_estimator_min_abs_dz_gate(self):
 
         A_cam_from_cv = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], dtype=np.float64)
         pose = CameraPoseField(
-            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=30.0, roll_deg=0.0),
+            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=-30.0, roll_deg=0.0),
             p_f=np.array([2.0, 1.0, 0.7], dtype=np.float64),
         )
 
@@ -367,7 +367,7 @@ def test_estimator_max_range_gate(self):
 
         A_cam_from_cv = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], dtype=np.float64)
         pose = CameraPoseField(
-            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=30.0, roll_deg=0.0),
+            R_f_c=R_field_from_cam_ypr(yaw_deg=0.0, pitch_deg=-30.0, roll_deg=0.0),
             p_f=np.array([2.0, 1.0, 0.7], dtype=np.float64),
         )