diff --git a/cereal b/cereal index 7e1d67d415..8af4582508 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7e1d67d4155651c6288e4b65fb7788871742e752 +Subproject commit 8af4582508dd7acef082c40ede11eb7018415d9a diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 42fa3b7c94..c9d9340a2d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/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 - (self.valid_blocks < INPUTS_NEEDED)) + height_std_threshold = MAX_HEIGHT_STD + 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) - 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) + + if (len(road_transform_trans) == 3): + new_height = np.array([road_transform_trans[2]]) + 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) diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index 18125cc43f..96f996413d 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/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) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 5538d6ff9b..6b0e626c5c 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/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); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 2b0902d5cf..1214c30062 100644 --- a/selfdrive/modeld/models/driving.h +++ b/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); diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 493f3285d6..53c1224ae8 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b8bf95f096b19cef1e473fb4f0caf5f727b74bbde23a642aa586036ad9824e55 -size 46076782 +oid sha256:4aa77af40335462062c6eb06632bb84cbc8e5c9e3ad759f66862711620e2a840 +size 46117948 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index e458f817d3..b89468eebb 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -91bdaeae5a7141ff6bb6fd80672521c8d63f644c +69270554b3c3ae574b9f357f2473395edf7db8af diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7fa8dd9530..c794a0434a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -73549898edd3d9a428fa6f58e25c2c0300290ef2 +dfe1e4a5fd7a464c91c0d2e70592b4b1470fda8d \ No newline at end of file