Revert "Use calibrator to stabilize wide transform (#26201)"

This reverts commit 1827b3c9e2942240f65d03ec0f790c91aa9c00ee.

old-commit-hash: 75b7fd0a7d

cereal

@@ -1 +1 @@
-Subproject commit 79e6e4c26dd12ba979a5fb872b4e68a707c32203
+Subproject commit 38133307b2e6036e76684b39878e79212e545e06

selfdrive/locationd/calibrationd.py

@@ -31,7 +31,6 @@ INPUTS_NEEDED = 5   # Minimum blocks needed for valid calibration
 INPUTS_WANTED = 50   # We want a little bit more than we need for stability
 MAX_ALLOWED_SPREAD = np.radians(2)
 RPY_INIT = np.array([0.0,0.0,0.0])
-WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
 
 # These values are needed to accommodate biggest modelframe
 PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657])
@@ -68,7 +67,6 @@ class Calibrator:
     calibration_params = params.get("CalibrationParams")
     self.wide_camera = params.get_bool('WideCameraOnly')
     rpy_init = RPY_INIT
-    wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
     valid_blocks = 0
 
     if param_put and calibration_params:
@@ -76,34 +74,24 @@ class Calibrator:
         msg = log.Event.from_bytes(calibration_params)
         rpy_init = np.array(msg.liveCalibration.rpyCalib)
         valid_blocks = msg.liveCalibration.validBlocks
-        wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler)
       except Exception:
         cloudlog.exception("Error reading cached CalibrationParams")
 
-    self.reset(rpy_init, valid_blocks, wide_from_device_euler)
+    self.reset(rpy_init, valid_blocks)
     self.update_status()
 
-  def reset(self, rpy_init: np.ndarray = RPY_INIT,
-                  valid_blocks: int = 0,
-                  wide_from_device_euler: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
-                  smooth_from: Optional[np.ndarray] = None) -> None:
+  def reset(self, rpy_init: np.ndarray = RPY_INIT, valid_blocks: int = 0, smooth_from: Optional[np.ndarray] = None) -> None:
     if not np.isfinite(rpy_init).all():
       self.rpy = RPY_INIT.copy()
     else:
       self.rpy = rpy_init.copy()
 
-    if not np.isfinite(wide_from_device_euler).all():
-      self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
-    else:
-      self.wide_from_device_euler = wide_from_device_euler.copy()
-
     if not np.isfinite(valid_blocks) or valid_blocks < 0:
       self.valid_blocks = 0
     else:
       self.valid_blocks = valid_blocks
 
     self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
-    self.wide_from_device_eulers = np.tile(wide_from_device_euler, (INPUTS_WANTED, 1))
 
     self.idx = 0
     self.block_idx = 0
@@ -127,8 +115,6 @@ class Calibrator:
     if valid_idxs:
       rpys = self.rpys[valid_idxs]
       self.rpy = np.mean(rpys, axis=0)
-      wide_from_device_eulers = self.wide_from_device_eulers[valid_idxs]
-      self.wide_from_device_euler = np.mean(wide_from_device_eulers, axis=0)
       max_rpy_calib = np.array(np.max(rpys, axis=0))
       min_rpy_calib = np.array(np.min(rpys, axis=0))
       self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
@@ -160,10 +146,7 @@ class Calibrator:
     else:
       return self.rpy
 
-  def handle_cam_odom(self, trans: List[float],
-                            rot: List[float],
-                            wide_from_device_euler: List[float],
-                            trans_std: List[float]) -> Optional[np.ndarray]:
+  def handle_cam_odom(self, trans: List[float], rot: List[float], trans_std: List[float]) -> Optional[np.ndarray]:
     self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
 
     straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
@@ -182,15 +165,7 @@ class Calibrator:
     new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
     new_rpy = sanity_clip(new_rpy)
 
-    if len(wide_from_device_euler):
-      new_wide_from_device_euler = np.array(wide_from_device_euler)
-    else:
-      new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
-
-    self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] +
-                                 (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
-    self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] +
-                                                    (BLOCK_SIZE - self.idx) * new_wide_from_device_euler) / float(BLOCK_SIZE)
+    self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
     self.idx = (self.idx + 1) % BLOCK_SIZE
     if self.idx == 0:
       self.block_idx += 1
@@ -212,7 +187,6 @@ class Calibrator:
     liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
     liveCalibration.rpyCalib = smooth_rpy.tolist()
     liveCalibration.rpyCalibSpread = self.calib_spread.tolist()
-    liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
 
     if self.not_car:
       liveCalibration.validBlocks = INPUTS_NEEDED
@@ -249,7 +223,6 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m
       calibrator.handle_v_ego(sm['carState'].vEgo)
       new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
                                            sm['cameraOdometry'].rot,
-                                           sm['cameraOdometry'].wideFromDeviceEuler,
                                            sm['cameraOdometry'].transStd)
 
       if DEBUG and new_rpy is not None:

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-a5c77655fba5563e8128fb655f69b1bbd02198aa
+f5a352666728344ca5065bb44c47c1f5650b4243