remove vp everywhere (#2122)

* remove vp everywhere

* wrong dimension

* useful disclaimer

* switcheroo

* imcomplete switcheroo

* did it wrong again

* difference look small enough

* fix comment

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, \
+from common.transformations.camera import get_view_frame_from_road_frame
-                                          get_calib_from_vp, vp_from_rpy, H, W, FOCAL
+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
+RPY_INIT = np.array([0,0,0])
-VP_INIT = np.array([W/2., H/2.])
 
 # 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):
+def is_calibration_valid(rpy):
-  return vp[0] > VP_VALIDITY_CORNERS[0, 0] and vp[0] < VP_VALIDITY_CORNERS[1, 0] and \
+  return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
-         vp[1] > VP_VALIDITY_CORNERS[0, 1] and vp[1] < VP_VALIDITY_CORNERS[1, 1]
 
 
-def sanity_clip(vp):
+def sanity_clip(rpy):
-  if np.isnan(vp).any():
+  if np.isnan(rpy).any():
-    vp = VP_INIT
+    rpy = RPY_INIT
-  return np.array([np.clip(vp[0], VP_VALIDITY_CORNERS[0, 0] - 5, VP_VALIDITY_CORNERS[1, 0] + 5),
+  return np.array([rpy[0],
-                   np.clip(vp[1], VP_VALIDITY_CORNERS[0, 1] - 5, VP_VALIDITY_CORNERS[1, 1] + 5)])
+                   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.rpy = copy.copy(RPY_INIT)
-    self.vps = np.zeros((INPUTS_WANTED, 2))
+    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"])
+        self.rpy = calibration_params["calib_radians"]
-        if not np.isfinite(self.vp).all():
+        if not np.isfinite(self.rpy).all():
-          self.vp = copy.copy(VP_INIT)
+          self.rpy = copy.copy(RPY_INIT)
-        self.vps = np.tile(self.vp, (INPUTS_WANTED, 1))
+        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
+      observed_rpy = np.array([0,
-      intrinsics = intrinsics_from_vp(self.vp)
+                               -np.arctan2(trans[2], trans[0]),
-      new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
+                               np.arctan2(trans[1], trans[0])])
-      new_vp = new_vp[:2]/new_vp[2]
+      new_rpy = euler_from_rot(rot_from_euler(self.rpy).dot(rot_from_euler(observed_rpy)))
-      new_vp = sanity_clip(new_vp)
+      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(self.rpy),
-        cal_params = {"calib_radians": list(calib),
                       "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)))
+      max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0))
-      min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0)))
+      min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0))
-      calib_spread = np.abs(max_vp_calib - min_vp_calib)
+      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:
+      if DEBUG and new_rpy is not None:
-        print('got new vp', new_vp)
+        print('got new rpy', new_rpy)
 
 
 def main(sm=None, pm=None):

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-eac7f0ebb37c2a8a0cf70d3d25d32ecd160d6cac
+24d46601f18d23f18e5152ce649fd0e37e2159a9