diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index a27e284a9c..7cca1c535f 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -1,4 +1,10 @@ #!/usr/bin/env python3 +''' +This process finds calibration values. More info on what these calibration values +are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations +While the roll calibration is a real value that can be estimated, here we assume it zero, +and the image input into the neural network is not corrected for roll. +''' import os import copy @@ -10,8 +16,8 @@ from selfdrive.locationd.calibration_helpers import Calibration from selfdrive.swaglog import cloudlog from common.params import Params, put_nonblocking from common.transformations.model import model_height -from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \ - get_calib_from_vp, vp_from_rpy, H, W, FOCAL +from common.transformations.camera import get_view_frame_from_road_frame +from common.transformations.orientation import rot_from_euler, euler_from_rot MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS MAX_VEL_ANGLE_STD = np.radians(0.25) @@ -19,40 +25,34 @@ MAX_YAW_RATE_FILTER = np.radians(2) # per second # This is all 20Hz, blocks needed for efficiency BLOCK_SIZE = 100 -INPUTS_NEEDED = 5 # allow to update VP every so many frames +INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability -WRITE_CYCLES = 10 # write every 1000 cycles -VP_INIT = np.array([W/2., H/2.]) +RPY_INIT = np.array([0,0,0]) # These values are needed to accomodate biggest modelframe -VP_VALIDITY_CORNERS = np.array([[W//2 - 63, 300], [W//2 + 63, 520]]) +PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657]) +YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235]) DEBUG = os.getenv("DEBUG") is not None -def is_calibration_valid(vp): - return vp[0] > VP_VALIDITY_CORNERS[0, 0] and vp[0] < VP_VALIDITY_CORNERS[1, 0] and \ - vp[1] > VP_VALIDITY_CORNERS[0, 1] and vp[1] < VP_VALIDITY_CORNERS[1, 1] +def is_calibration_valid(rpy): + return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) -def sanity_clip(vp): - if np.isnan(vp).any(): - vp = VP_INIT - return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0, 0] - 5, VP_VALIDITY_CORNERS[1, 0] + 5), - np.clip(vp[1], VP_VALIDITY_CORNERS[0, 1] - 5, VP_VALIDITY_CORNERS[1, 1] + 5)]) +def sanity_clip(rpy): + if np.isnan(rpy).any(): + rpy = RPY_INIT + return np.array([rpy[0], + 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 intrinsics_from_vp(vp): - return np.array([ - [FOCAL, 0., vp[0]], - [ 0., FOCAL, vp[1]], - [ 0., 0., 1.]]) - class Calibrator(): def __init__(self, param_put=False): self.param_put = param_put - self.vp = copy.copy(VP_INIT) - self.vps = np.zeros((INPUTS_WANTED, 2)) + self.rpy = copy.copy(RPY_INIT) + self.rpys = np.zeros((INPUTS_WANTED, 3)) self.idx = 0 self.block_idx = 0 self.valid_blocks = 0 @@ -68,10 +68,10 @@ class Calibrator(): if calibration_params: try: calibration_params = json.loads(calibration_params) - self.vp = vp_from_rpy(calibration_params["calib_radians"]) - if not np.isfinite(self.vp).all(): - self.vp = copy.copy(VP_INIT) - self.vps = np.tile(self.vp, (INPUTS_WANTED, 1)) + 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 @@ -84,7 +84,7 @@ class Calibrator(): if self.valid_blocks < INPUTS_NEEDED: self.cal_status = Calibration.UNCALIBRATED else: - self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID + self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.rpy) else Calibration.INVALID end_status = self.cal_status self.just_calibrated = False @@ -99,47 +99,45 @@ class Calibrator(): certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or (self.valid_blocks < INPUTS_NEEDED)) if straight_and_fast and certain_if_calib: - # intrinsics are not eon intrinsics, since this is calibrated frame - intrinsics = intrinsics_from_vp(self.vp) - new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans)) - new_vp = new_vp[:2]/new_vp[2] - new_vp = sanity_clip(new_vp) + 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 = sanity_clip(new_rpy) - self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / 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 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED if self.valid_blocks > 0: - self.vp = np.mean(self.vps[:self.valid_blocks], axis=0) + self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0) self.update_status() if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated): - calib = get_calib_from_vp(self.vp) - cal_params = {"calib_radians": list(calib), + cal_params = {"calib_radians": list(self.rpy), "valid_blocks": self.valid_blocks} put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) - return new_vp + return new_rpy else: return None def send_data(self, pm): - calib = get_calib_from_vp(self.vp) if self.valid_blocks > 0: - max_vp_calib = np.array(get_calib_from_vp(np.max(self.vps[:self.valid_blocks], axis=0))) - min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0))) - calib_spread = np.abs(max_vp_calib - min_vp_calib) + 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, calib[1], calib[2], model_height) + extrinsic_matrix = get_view_frame_from_road_frame(0, self.rpy[1], self.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 calib] + cal_send.liveCalibration.rpyCalib = [float(x) for x in self.rpy] cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread] pm.send('liveCalibration', cal_send) @@ -170,13 +168,13 @@ def calibrationd_thread(sm=None, pm=None): send_counter += 1 if sm.updated['cameraOdometry']: - new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, + new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, sm['cameraOdometry'].rot, sm['cameraOdometry'].transStd, sm['cameraOdometry'].rotStd) - if DEBUG and new_vp is not None: - print('got new vp', new_vp) + if DEBUG and new_rpy is not None: + print('got new rpy', new_rpy) def main(sm=None, pm=None): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b5db61df15..b893603db8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -eac7f0ebb37c2a8a0cf70d3d25d32ecd160d6cac \ No newline at end of file +24d46601f18d23f18e5152ce649fd0e37e2159a9 \ No newline at end of file