hot coffee model (#28296)

* 1061b1c7-b944-43e3-a940-b56b64d66f54/700

* bump cereal

* bump cereal

* make mypy happy

* write TODO

* read height from written params

* fix certain_if_calib logic and unit tests

* factor moving_avg_with_linear_decay

* remove whitespace

* update model ref commit

* default value for CI

* update process replay ref commit

* update process replay ref commit

* update process replay ref commit

* bump cereal

cereal

@@ -1 +1 @@
-Subproject commit 7e1d67d4155651c6288e4b65fb7788871742e752
+Subproject commit 8af4582508dd7acef082c40ede11eb7018415d9a

selfdrive/locationd/calibrationd.py

@@ -24,6 +24,8 @@ MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
 MAX_VEL_ANGLE_STD = np.radians(0.25)
 MAX_YAW_RATE_FILTER = np.radians(2)  # per second
 
+MAX_HEIGHT_STD = np.exp(-3.5)
+
 # This is at model frequency, blocks needed for efficiency
 SMOOTH_CYCLES = 10
 BLOCK_SIZE = 100
@@ -32,6 +34,7 @@ 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])
+HEIGHT_INIT = np.array([1.22])
 
 # These values are needed to accommodate the model frame in the narrow cam of the C3
 PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
@@ -50,6 +53,8 @@ def sanity_clip(rpy: np.ndarray) -> np.ndarray:
                    np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
                    np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
 
+def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
+  return (idx*prev_mean + (block_size - idx) * new_val) / block_size
 
 class Calibrator:
   def __init__(self, param_put: bool = False):
@@ -62,6 +67,7 @@ class Calibrator:
     calibration_params = params.get("CalibrationParams")
     rpy_init = RPY_INIT
     wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
+    height = HEIGHT_INIT
     valid_blocks = 0
     self.cal_status = log.LiveCalibrationData.Status.uncalibrated
 
@@ -71,21 +77,28 @@ class Calibrator:
         rpy_init = np.array(msg.liveCalibration.rpyCalib)
         valid_blocks = msg.liveCalibration.validBlocks
         wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler)
+        height = np.array(msg.liveCalibration.height)
       except Exception:
         cloudlog.exception("Error reading cached CalibrationParams")
 
-    self.reset(rpy_init, valid_blocks, wide_from_device_euler)
+    self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
     self.update_status()
 
   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,
+                  height_init: np.ndarray = HEIGHT_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(height_init).all() or len(height_init) != 1:
+      self.height = HEIGHT_INIT.copy()
+    else:
+      self.height = height_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:
@@ -98,6 +111,7 @@ class Calibrator:
 
     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.heights = np.tile(self.height, (INPUTS_WANTED, 1))
 
     self.idx = 0
     self.block_idx = 0
@@ -120,6 +134,7 @@ class Calibrator:
     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)
+      self.height = np.mean(self.heights[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))
@@ -140,6 +155,7 @@ class Calibrator:
 
     # If spread is too high, assume mounting was changed and reset to last block.
     # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
+    # TODO: add height spread check with smooth transition too
     if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated:
       self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
       self.cal_status = log.LiveCalibrationData.Status.recalibrating
@@ -160,13 +176,21 @@ class Calibrator:
   def handle_cam_odom(self, trans: List[float],
                             rot: List[float],
                             wide_from_device_euler: List[float],
-                            trans_std: List[float]) -> Optional[np.ndarray]:
+                            trans_std: List[float],
+                            road_transform_trans: List[float],
+                            road_transform_trans_std: List[float]) -> Optional[np.ndarray]:
     self.old_rpy_weight = max(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))
     angle_std_threshold = MAX_VEL_ANGLE_STD
-    certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or
+    height_std_threshold = MAX_HEIGHT_STD
-                        (self.valid_blocks < INPUTS_NEEDED))
+    rpy_certain = np.arctan2(trans_std[1], trans[0]) < angle_std_threshold
+    if len(road_transform_trans_std) == 3:
+      height_certain = road_transform_trans_std[2] < height_std_threshold
+    else:
+      height_certain = True
+
+    certain_if_calib = (rpy_certain and height_certain) or (self.valid_blocks < INPUTS_NEEDED)
     if not (straight_and_fast and certain_if_calib):
       return None
 
@@ -180,10 +204,16 @@ class Calibrator:
       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)
+    if (len(road_transform_trans) == 3):
-    self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] +
+      new_height = np.array([road_transform_trans[2]])
-                                                    (BLOCK_SIZE - self.idx) * new_wide_from_device_euler) / float(BLOCK_SIZE)
+    else:
+      new_height = HEIGHT_INIT
+
+    self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
+    self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx], new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
+    self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
+
     self.idx = (self.idx + 1) % BLOCK_SIZE
     if self.idx == 0:
       self.block_idx += 1
@@ -206,6 +236,7 @@ class Calibrator:
     liveCalibration.rpyCalib = smooth_rpy.tolist()
     liveCalibration.rpyCalibSpread = self.calib_spread.tolist()
     liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
+    liveCalibration.height = self.height.tolist()
 
     if self.not_car:
       liveCalibration.validBlocks = INPUTS_NEEDED
@@ -243,7 +274,9 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m
       new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
                                            sm['cameraOdometry'].rot,
                                            sm['cameraOdometry'].wideFromDeviceEuler,
-                                           sm['cameraOdometry'].transStd)
+                                           sm['cameraOdometry'].transStd,
+                                           sm['cameraOdometry'].roadTransformTrans,
+                                           sm['cameraOdometry'].roadTransformTransStd)
 
       if DEBUG and new_rpy is not None:
         print('got new rpy', new_rpy)

selfdrive/locationd/test/test_calibrationd.py

@@ -7,7 +7,7 @@ import numpy as np
 import cereal.messaging as messaging
 from cereal import log
 from common.params import Params
-from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES
+from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT
 
 
 class TestCalibrationd(unittest.TestCase):
@@ -16,10 +16,12 @@ class TestCalibrationd(unittest.TestCase):
     msg = messaging.new_message('liveCalibration')
     msg.liveCalibration.validBlocks = random.randint(1, 10)
     msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)]
+    msg.liveCalibration.height = [random.random() for _ in range(1)]
     Params().put("CalibrationParams", msg.to_bytes())
     c = Calibrator(param_put=True)
 
     np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy)
+    np.testing.assert_allclose(msg.liveCalibration.height, c.height)
     self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks)
 
 
@@ -27,51 +29,79 @@ class TestCalibrationd(unittest.TestCase):
     c = Calibrator(param_put=False)
     for _ in range(BLOCK_SIZE * INPUTS_WANTED):
       c.handle_v_ego(MIN_SPEED_FILTER + 1)
-      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e-3, 1e-3, 1e-3])
     self.assertEqual(c.valid_blocks, INPUTS_WANTED)
     np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
     c.reset()
 
+
   def test_calibration_low_speed_reject(self):
     c = Calibrator(param_put=False)
     for _ in range(BLOCK_SIZE * INPUTS_WANTED):
       c.handle_v_ego(MIN_SPEED_FILTER - 1)
-      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e-3, 1e-3, 1e-3])
     for _ in range(BLOCK_SIZE * INPUTS_WANTED):
       c.handle_v_ego(MIN_SPEED_FILTER + 1)
-      c. handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0],
+      c.handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e-3, 1e-3, 1e-3])
     self.assertEqual(c.valid_blocks, 0)
     np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
 
 
   def test_calibration_yaw_rate_reject(self):
     c = Calibrator(param_put=False)
     for _ in range(BLOCK_SIZE * INPUTS_WANTED):
       c.handle_v_ego(MIN_SPEED_FILTER + 1)
-      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
                          [0.0, 0.0, MAX_YAW_RATE_FILTER ],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e-3, 1e-3, 1e-3])
     self.assertEqual(c.valid_blocks, 0)
     np.testing.assert_allclose(c.rpy, np.zeros(3))
+    np.testing.assert_allclose(c.height, HEIGHT_INIT)
 
 
   def test_calibration_speed_std_reject(self):
     c = Calibrator(param_put=False)
     for _ in range(BLOCK_SIZE * INPUTS_WANTED):
       c.handle_v_ego(MIN_SPEED_FILTER + 1)
-      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e3, 1e3, 1e3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
+                         [1e-3, 1e-3, 1e-3])
+    self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_speed_std_height_reject(self):
+    c = Calibrator(param_put=False)
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e3, 1e3, 1e3])
     self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
     np.testing.assert_allclose(c.rpy, np.zeros(3))
@@ -81,9 +111,11 @@ class TestCalibrationd(unittest.TestCase):
     c = Calibrator(param_put=False)
     for _ in range(BLOCK_SIZE * INPUTS_WANTED):
       c.handle_v_ego(MIN_SPEED_FILTER + 1)
-      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e-3, 1e-3, 1e-3])
     self.assertEqual(c.valid_blocks, INPUTS_WANTED)
     np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
@@ -95,6 +127,8 @@ class TestCalibrationd(unittest.TestCase):
       c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0],
                          [0.0, 0.0, 0.0],
                          [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3],
+                         [0.0, 0.0, HEIGHT_INIT.item()],
                          [1e-3, 1e-3, 1e-3])
     self.assertEqual(c.valid_blocks, 1)
     self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating)

selfdrive/modeld/models/driving.cc

@@ -390,15 +390,18 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_droppe
   const auto &v_std = net_outputs.pose.velocity_std;
   const auto &r_std = net_outputs.pose.rotation_std;
   const auto &t_std = net_outputs.wide_from_device_euler.std;
+  const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean;
+  const auto &road_transform_trans_std = net_outputs.road_transform.position_std;
 
   auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry();
   posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
   posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
   posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z});
+  posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z});
   posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
   posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
   posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)});
-
+  posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)});
 
   posenetd.setTimestampEof(timestamp_eof);
   posenetd.setFrameId(vipc_frame_id);

selfdrive/modeld/models/driving.h

@@ -178,6 +178,14 @@ struct ModelOutputTemporalPose {
 };
 static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4);
 
+struct ModelOutputRoadTransform {
+  ModelOutputXYZ position_mean;
+  ModelOutputXYZ rotation_mean;
+  ModelOutputXYZ position_std;
+  ModelOutputXYZ rotation_std;
+};
+static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4);
+
 struct ModelOutputDisengageProb {
   float gas_disengage;
   float brake_disengage;
@@ -237,6 +245,7 @@ struct ModelOutput {
   const ModelOutputPose pose;
   const ModelOutputWideFromDeviceEuler wide_from_device_euler;
   const ModelOutputTemporalPose temporal_pose;
+  const ModelOutputRoadTransform road_transform;
 };
 
 constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:b8bf95f096b19cef1e473fb4f0caf5f727b74bbde23a642aa586036ad9824e55
+oid sha256:4aa77af40335462062c6eb06632bb84cbc8e5c9e3ad759f66862711620e2a840
-size 46076782
+size 46117948

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-91bdaeae5a7141ff6bb6fd80672521c8d63f644c
+69270554b3c3ae574b9f357f2473395edf7db8af

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-73549898edd3d9a428fa6f58e25c2c0300290ef2
+dfe1e4a5fd7a464c91c0d2e70592b4b1470fda8d