Calibrationd: Calibrate wide from device (#26204)

* Use calibrator to stabilize wide transform

* Need array

* Needs init

* Needs to be arr

* publish mshg

* Check size

* Update ref

* fix calibration param read when no wide calib

* need self!

selfdrive/locationd/calibrationd.py

@@ -31,6 +31,7 @@ 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])
@@ -67,6 +68,7 @@ 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:
@@ -74,24 +76,34 @@ 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)
+    self.reset(rpy_init, valid_blocks, wide_from_device_euler)
     self.update_status()
 
-  def reset(self, rpy_init: np.ndarray = RPY_INIT, valid_blocks: int = 0, smooth_from: Optional[np.ndarray] = None) -> None:
+  def reset(self, rpy_init: np.ndarray = RPY_INIT,
+                  valid_blocks: int = 0,
+                  wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
+                  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_init).all() or len(wide_from_device_euler_init) != 3:
+      self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
+    else:
+      self.wide_from_device_euler = wide_from_device_euler_init.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(self.wide_from_device_euler, (INPUTS_WANTED, 1))
 
     self.idx = 0
     self.block_idx = 0
@@ -113,6 +125,7 @@ class Calibrator:
   def update_status(self) -> None:
     valid_idxs = self.get_valid_idxs()
     if valid_idxs:
+      self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
       rpys = self.rpys[valid_idxs]
       self.rpy = np.mean(rpys, axis=0)
       max_rpy_calib = np.array(np.max(rpys, axis=0))
@@ -146,7 +159,10 @@ class Calibrator:
     else:
       return self.rpy
 
-  def handle_cam_odom(self, trans: List[float], rot: List[float], trans_std: List[float]) -> Optional[np.ndarray]:
+  def handle_cam_odom(self, trans: List[float],
+                            rot: List[float],
+                            wide_from_device_euler: 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))
@@ -165,7 +181,15 @@ 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)
 
-    self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
+    if len(wide_from_device_euler) == 3:
+      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.idx = (self.idx + 1) % BLOCK_SIZE
     if self.idx == 0:
       self.block_idx += 1
@@ -187,6 +211,7 @@ 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
@@ -223,6 +248,7 @@ 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 @@
-f5a352666728344ca5065bb44c47c1f5650b4243
+a5c77655fba5563e8128fb655f69b1bbd02198aa