From d36675e6184bd45c06b84658c445bac7560a2f6e Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 14 Sep 2020 04:02:58 -0700 Subject: [PATCH] Auto reset bad calibration (#2151) * initial commit * thanks pylint * smoooooth * cleanup * cleaner * switched * always decay weight old-commit-hash: df99316621ae51870a92fdc767ec4651261b7314 --- selfdrive/locationd/calibrationd.py | 110 +++++++++++++++++++--------- 1 file changed, 74 insertions(+), 36 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 3efffea053..db0bcec6c7 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -22,22 +22,26 @@ 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 -# This is all 20Hz, blocks needed for efficiency +# This is at model frequency, blocks needed for efficiency +SMOOTH_CYCLES = 400 BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability -RPY_INIT = np.array([0,0,0]) +MAX_ALLOWED_SPREAD = np.radians(2) +RPY_INIT = np.array([0.0,0.0,0.0]) # These values are needed to accomodate biggest modelframe PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657]) YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235]) DEBUG = os.getenv("DEBUG") is not None + class Calibration: UNCALIBRATED = 0 CALIBRATED = 1 INVALID = 2 + def is_calibration_valid(rpy): return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) @@ -53,43 +57,84 @@ def sanity_clip(rpy): class Calibrator(): def __init__(self, param_put=False): self.param_put = param_put - self.rpy = copy.copy(RPY_INIT) - self.rpys = np.zeros((INPUTS_WANTED, 3)) - self.idx = 0 - self.block_idx = 0 - self.valid_blocks = 0 - self.cal_status = Calibration.UNCALIBRATED - self.just_calibrated = False - self.v_ego = 0 # Read saved calibration calibration_params = Params().get("CalibrationParams") + + rpy_init = RPY_INIT + valid_blocks = 0 + if param_put and calibration_params: try: calibration_params = json.loads(calibration_params) - self.rpy = calibration_params["calib_radians"] - if not np.isfinite(self.rpy).all(): - self.rpy = copy.copy(RPY_INIT) - self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) - self.valid_blocks = calibration_params['valid_blocks'] - if not np.isfinite(self.valid_blocks) or self.valid_blocks < 0: - self.valid_blocks = 0 - self.update_status() + rpy_init = calibration_params["calib_radians"] + valid_blocks = calibration_params['valid_blocks'] except Exception: cloudlog.exception("CalibrationParams file found but error encountered") + self.reset(rpy_init, valid_blocks) + self.update_status() + + def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None): + if not np.isfinite(rpy_init).all(): + self.rpy = copy.copy(RPY_INIT) + else: + self.rpy = rpy_init + 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.idx = 0 + self.block_idx = 0 + self.v_ego = 0 + + if smooth_from is None: + self.old_rpy = RPY_INIT + self.old_rpy_weight = 0.0 + else: + self.old_rpy = smooth_from + self.old_rpy_weight = 1.0 def update_status(self): - start_status = self.cal_status + if self.valid_blocks > 0: + max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0)) + min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0)) + self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib) + else: + self.calib_spread = np.zeros(3) + if self.valid_blocks < INPUTS_NEEDED: self.cal_status = Calibration.UNCALIBRATED + elif is_calibration_valid(self.rpy): + self.cal_status = Calibration.CALIBRATED else: - self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.rpy) else Calibration.INVALID - self.just_calibrated = start_status == Calibration.UNCALIBRATED and self.cal_status != Calibration.UNCALIBRATED + self.cal_status = Calibration.INVALID + + # If spread is too high, assume mounting was changed and reset to last block. + # Make the transition smooth. Abrupt transistion are not good foor feedback loop through supercombo model. + if max(self.calib_spread) > MAX_ALLOWED_SPREAD: + self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy) + + write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) + if self.param_put and write_this_cycle: + # TODO: this should use the liveCalibration struct from cereal + cal_params = {"calib_radians": list(self.rpy), + "valid_blocks": int(self.valid_blocks)} + put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) def handle_v_ego(self, v_ego): self.v_ego = v_ego + def get_smooth_rpy(self): + if self.old_rpy_weight > 0: + return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy + else: + return self.rpy + def handle_cam_odom(self, trans, rot, trans_std, rot_std): + 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)) certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or (self.valid_blocks < INPUTS_NEEDED)) @@ -97,7 +142,7 @@ class Calibrator(): observed_rpy = np.array([0, -np.arctan2(trans[2], trans[0]), np.arctan2(trans[1], trans[0])]) - new_rpy = euler_from_rot(rot_from_euler(self.rpy).dot(rot_from_euler(observed_rpy))) + 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) @@ -108,33 +153,25 @@ class Calibrator(): self.block_idx = self.block_idx % INPUTS_WANTED if self.valid_blocks > 0: self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0) + + self.update_status() - # TODO: this should use the liveCalibration struct from cereal - if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated): - cal_params = {"calib_radians": list(self.rpy), - "valid_blocks": self.valid_blocks} - put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) return new_rpy else: return None def send_data(self, pm): - if self.valid_blocks > 0: - max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0)) - min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0)) - calib_spread = np.abs(max_rpy_calib - min_rpy_calib) - else: - calib_spread = np.zeros(3) - extrinsic_matrix = get_view_frame_from_road_frame(0, self.rpy[1], self.rpy[2], model_height) + smooth_rpy = self.get_smooth_rpy() + extrinsic_matrix = get_view_frame_from_road_frame(0, smooth_rpy[1], smooth_rpy[2], model_height) cal_send = messaging.new_message('liveCalibration') cal_send.liveCalibration.validBlocks = self.valid_blocks cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] - cal_send.liveCalibration.rpyCalib = [float(x) for x in self.rpy] - cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread] + cal_send.liveCalibration.rpyCalib = [float(x) for x in smooth_rpy] + cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in self.calib_spread] pm.send('liveCalibration', cal_send) @@ -166,6 +203,7 @@ def calibrationd_thread(sm=None, pm=None): if sm.frame % 5 == 0: calibrator.send_data(pm) + def main(sm=None, pm=None): calibrationd_thread(sm, pm)