Auto reset bad calibration (#2151)
* initial commit * thanks pylint * smoooooth * cleanup * cleaner * switched * always decay weightpull/2172/head
HaraldSchafer
GitHub
parent
272f60221c
commit
df99316621
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue