mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-22 14:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

sanity clip every vp

Browse Source
This commit is contained in:
Harald Schafer
2020-01-22 13:09:03 -08:00
parent c32ebcb2c6
commit ba6fd511b6
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+2 -2
View File
@@ -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):