From 88c754975f5659cadb09814fb3cc244cea1df5b4 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Wed, 22 Jan 2020 13:09:03 -0800 Subject: [PATCH] sanity clip every vp old-commit-hash: ba6fd511b686acc4df674f37b02f5a5209710f4d --- selfdrive/locationd/calibrationd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index cd92a5f63..a72177909 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -96,6 +96,7 @@ class Calibrator(): 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) self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE) self.idx = (self.idx + 1) % BLOCK_SIZE @@ -103,8 +104,7 @@ class Calibrator(): self.block_idx += 1 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED - raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0) - self.vp = sanity_clip(raw_vp) + self.vp = np.mean(self.vps[:max(1, 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):