diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 3757a0b8a1..bc0f0a0396 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -14,13 +14,13 @@ from common.transformations.camera import view_frame_from_device_frame, get_view MPH_TO_MS = 0.44704 MIN_SPEED_FILTER = 15 * MPH_TO_MS -MAX_SPEED_STD = 1.5 +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 BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # allow to update VP every so many frames -INPUTS_WANTED = 20 # We want a little bit more than we need for stability +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.]) @@ -89,9 +89,10 @@ class Calibrator(): self.just_calibrated = True def handle_cam_odom(self, trans, rot, trans_std, rot_std): - if ((trans[0] > MIN_SPEED_FILTER) and - (trans_std[0] < MAX_SPEED_STD) and - (abs(rot[2]) < MAX_YAW_RATE_FILTER)): + straight_and_fast = ((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)) + 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)) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5f3d060951..150649e671 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7bc2ec3b0888827c0028bc7b35ede111ba2b0419 \ No newline at end of file +b98f4b6858a21fd6602fb41c6a21f69b1ca81921 \ No newline at end of file